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ABSTRACT 


/‘Autonomous  Underwater  Vehicles  (AUV)  are  being 
considered  by  the  U.S.  Navy  for  a  variety  of  missions. 
Requirements  for  autonomy  reinforce  the  need  for  a  robust 
maneuvering  controller  that  can  ensure  accurate  tracking  of 
a  planned  path.  Model  reference  controllers  (MRC)  have  been 
employed  in  situations  where  accurate  tracking  is  desired 
and  where  plant  parameters  change  with  operating  conditions. 
Because  underwater  vehicles  are  highly  non-linear,  it  is 
conjectured  that  an  MRC  will  provide  improved  tracking 
performance  for  AUVs.  This  thesis  presents  the  results  of  a 
simulation  study  in  which  the  dynamics  of  a  submersible  are 
modeled  using  a  modified  version  of  the  DTNSRDC  2510 
equations  of  motion.  A  linearized  version  of  these 
equations  serves  as  the  reference  model  and  provides  the 
basis  for  the  design  of  feedforward  and  feedback  elements  of 
the  controller.  Results  show  that  C  )r  dive  plane  maneuvers, 
accurate  tracking  of  the  planned  p&rh  can  be  achieved  for  a 
moderately  wide  range  of  vehicle  speeds. 
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A.  GENERAL 


In  recent  years,  the  focus  on  Autonomous  Underwater 
Vehicles  (AUV's)  or,  mere  generally,  Unmanned  Underwater 
Vehicles  (UUV's)  has  increased.  A  variety  of  unclassified 
missions  includes  Search  and  Survey,  Decoy  and  Outboard 
sensors.  Ocean  Engineering  Work  Service,  Swimmer  Support, 
and  Test  and  Evaluations  [Ref.  1] .  As  the  cost  of  manned 
submarine  vehicles  increases,  there  are  significant 
advantages  to  the  use  of  cheaper  unmanned  vehicles.  UUV's 
can  be  either  tethered  or  untethered.  Development  in  both 
areas  is  proceeding,  but,  while  tethered  vehicles  can  use 
fiber  optic  links  to  human  operators  on  a  mother  ship,  a 
fully  autonomous  vehicle  is  required  to  have  a  high  level  of 
intelligent  processing  on  board.  Thus  the  requirements  for 
AI  and  Knowledge-Based  Controls  are  much  increased.  A 
recent  symposium  [Ref.  2]  has  presented  a  summary  of  the 
State  of  the  Art  in  Unmanned  Untethered  Submersible 
Technology. 

The  organization  of  the  intelligent  control  of  an  AUV 
can  be  expressed  as  a  cycle  of  Sensing,  Thinking,  and  Acting 
(Figure  1.1).  At  the  highest  level  of  the  control 
architecture,  the  mission  planning  and  symbolic  reasoning 
lead  to  requirements  for  path  planning  and  control.  The 
lower  level  of  Acting  involves  operation  and  control  of  all 
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Figure  1.1  Organization  of  Intelligent  Control 


vehicle  modes  of  behavior.  At  the  Sensing  level,  all 
information  concerning  the  environment  surrounding  the 
vehicle,  as  well  as  its  own  internal  state  of  health,  is 
directed  to  the  higher  level.  Figure  1.1,  reproduced  here 
from  [Ref.  2]  illustrates  the  idea,  and  Figure  1.2 
illustrates  the  hierarchical  nature  of  the  intelligent- 
controls  required. 

Part  of  the  sensing  and  reflexive  acting  at  the  lowest 
level  involves  a  high  degree  of  servo-control  over  all  six 
degrees  of  freedom  of  the  vehicle  motion.  To  effect  proper 
control,  not  only  must  the  autopilot  be  capable  of  accurate 
course  and  depth  control,  but  also,  commands  for  reflexive 
actions  for  avoidance  or  attack  must  be  followed  accurately 
These  commands  can  also  include  hovering  while  some  form  of 
underwater  work  is  done. 

B.  AIM  OF  THE  STUDY 

This  thesis  is  concerned  with  the  lowest  level  of 
control — the  control  of  vehicle  reflexive  maneuvers.  It  is 
assumed  that  the  planning  level  control  in  Figure  1.2 
recognizes  the  need  for  evasive  action  and  decides  on 
parameters  such  as  speed,  course,  and  depth  changes  to  be 
rapidly  implemented.  These  parameters  are  then  fed  to  a 
series  of  stored  maneuvers  within  the  framework  of  a  model 
based  autopilot  system.  Figure  1.3  illustrates  the  concept 
of  the  "bag”  of  maneuvers  as  interfaced  to  the  vehicle 
autopilot.  The  control  concept  proposed  here  is  that  of  a 
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Figure  1.3  Schematic  of  Supervisory  Type  Control  Scheme 
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developed  for  such  maneuvers  can  be  in  the  form  of 
algorithms  that  provide  a  command  generation  system  to  the 
autopilot. 

The  purpose  of  this  work  is  to  determine  the  feasibility 
and  the  autopilot  design  methodology  for: 

1.  the  command  generation  logic,  and 

2.  a  model  following  autopilot  control. 

C.  METHOD  OF  APPROACH 

Since  this  work  deals  strongly  with  underwater  vehicle 
dynamics  and  control,  but  not  with  the  vehicle  hydrodynamics 
per  se,  it  was  important  to  use  an  existing  vehicle  model  as 
the  basis  for  the  work.  Such  a  model  (Figure  1.4)  was 
provided  by  [Ref.  4]  where  the  verification  of  the  model  by 
experimental  data  illustrated  the  reasonableness  of  its 
coefficients. 

Using  the  equations  of  motions  of  the  vehicle,  the 
development  of  command  generation  logic,  the  design  of  the 
model  following  autopilot,  and  the  AUV  maneuvering 
performance,  have  been  accomplished  with  compulsr 
simulation.  Heavy  use  of  the  DSL  (Dynamic  Simulation 
Language)  has  been  made.  [Ref.  5] 

The  vehicle  selected  as  the  basis  for  the  study  is 
approximately  17  feet  long  and  has  been  simulated  over  a 
range  of  speeds  from  3  to  30  feet  per  second  where  a 


**• 

> 

3 


specific  maneuver — a  rapid  dive  to  100  feet — has  been  the 
focus  for  the  command  generation  model. 

While  much  remains  to  be  done,  the  concept  proposed 
appears  worthy  of  future  work. 
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II.  VEHICLE  DYNAMIC  MODELING 


A.  GENERAL 

This  chapter  describes  the  dynamics  of  a  selected  AUV. 
The  three  dimensional  motion  of  an  underwater  vehicle  is 
fully  defined  using  two  coordinate  reference  systems. 

1.  Body  Fixed  Coordinate  Reference  System— Figure  2.1. 

2 .  Inertial  Reference  System — Figure  2.2. 

The  vehicle  equations  of  motion  are  presented  and  how 
they  were  modified  to  suit  the  needs  of  an  AUV.  Also 
included  as  part  of  this  chapter  is  a  description  of  the 
derivation  of  the  hydrodynamic  coefficients  and  a  brief 
discussion  of  the  propulsion  plant  and  crossflow  drag 
modeling. 

B.  COORDINATE  SYSTEMS 

Three  dimensional  motions  of  underwater  vehicles  are 
normally  described  using  two  coordinate  reference  frames. 

The  first  is  a  right-handed  orthogonal  coordinate  system 
fixed  in  the  body.  The  second,  an  ineri  !.al  reference  frame, 
is  used  to  define  translational  and  rotational  motions  in 
global  coordinates  (Figure  2.1) 

The  body  fixed  coordinate  reference  frame  has  its  origin 
fixed  to  the  body  center  and  is  aligned  with  the  body  axis 
of  symmetry.  Components  of  the  vehicle  motion  relative  to 
this  body  fixed  reference  frame  are  defined  as: 
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u,v,w  components  along  the  body  fixed  axaa  of  the 
velocity  of  the  origin  relative  to  the  fluid 
(Surge,  Sway  and  Haava  velocity  raapactivaly) . 

p,q,r  components  along  the  body  fixed  axes  of  the 
angular  velocity  of  body  relative  to  the 
inertial  reference  system  (Roll,  Pitch,  and  Yaw 
rotes)  (Figure  2.2). 

The  inertial  reference  frame  is  also  a  right  handed 
orthogonal  coordinate  system  in  which  the  position  and 
orientation  of  the  vehicle's  coordinate  system  is  specified. 
The  orientation  of  the  body-fixed  coordinate  system  is 
described  by  Euler  angles  <p  (yaw) ,  6  (pitch)  ,  <b  (roll)  . 

The  transformation  from  body-fixed  to  inertial  is  then  given 
conveniently  by  an  XYZ  rotation  sequence  (<f>,  ex¬ 
position  of  the  body-fixed  coordinate  system  is  then 
expressed  in  X,  Y,  and  Z  coordinates  as  illustrated  in 
Figure  2.1.  Orientation  of  the  vehicle's  coordinate  system 
is  expressed  in  Euler  angles  <j>,  e#  ip  ■ 

C.  RIGID  BODY  DYNAMICS  AND  EQUATIONS  OF  MOTION 

The  equations  of  motion  for  a  six  degree  of  freedom 
submarine  vehicle  are  now  standardized  being  first  fully 
developed  by  Gertler  and  Hagen  [Ref.  6].  These  equations 
are  commonly  known  today  as  the  DTNSRDC  2510  equations  of 
motion. 

Modifications  to  these  standard  equations  are  then 
generally  made  to  reflect  the  particular  hydrodynamic 
characteristics  and  properties  of  the  underwater  vehicle 
being  considered.  [Ref.  5]  Among  the  most  significant 
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changes/allowances  considered  for  the  AUV  in  this  study 


included  an  integral  formulation  of  the  viscous  crossflow 


forces  and  moments,  addition  of  the  effect  of  an  external 


current  and  perhaps  the  most  significant  difference  is  the 


change  made  due  to  the  non-convent ional  shape  of  the  AUV. 


The  AUV  considered  here  is  peculiar  in  that  its  shape  is 


more  of  low  aspect  ratio  wing  than  that  of  the  conventional 


body  of  revolution.  [Ref.  4]  Additional  modifications  were 


also  made  by  the  separation  of  the  coupled  input  for  bow  and 


stern  planes  and  also  the  decoupling  of  the  bow  planes  so 


that  purposely  induced  roll  control  could  be  included. 


The  equations  of  motion  for  the  six  degree  of  freedom 


AUV  are  listed  in  Appendix  A,  in  the  following  form: 


MX*  £(X,i,ll) 


(2.1) 


where , 


M  »  MASS  MATRIX 


(2.2) 


X  =  [u,v,w,p,q,r] 


(2.3) 


£  -  [Fx,Fy,Fz,K,M,N]' 


(2.4) 


and  Fx,  Fy,  Fz  are  hydrodynamic  forces  and  K,  M,  N  are 
hydrodynamic  moments, 


u  “ 

surge 

V 

sway 

V 

-  heave 

p 

roll 

q 

pitch 

r 

■  .  » 

yaw 

-  Body  Coordinate  States 


(2.5) 


a 


x 

Y 

Z 

♦ 

e 

«r  ‘ 

^bp 

«s 

^rm 

% 


Position  n 


Orientation! 


(2.6) 

}-  Inertial  Reference  System 


rudder  angle 
ST&D  bow  plane  angle 
port  bow  plane  angle 
stern  plane  angle 
delta  form 
delta  buoyancy 


control 

vector 


(2.7) 


and  u  is  distinguished  by  context  from  u — surge  velocity  of 
the  vehicle  relative  to  the  surrounding  water,  or  Uco  for 
the  current. 

In  addition  to  the  six  equations  of  motion  that  define 
the  AUV's  motion  relative  to  the  body  fixed  coordinate 
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system,  six  additional  equations  are  required  to  fully 
specify  the  vehicle's  motion  in  space.  These  kinematic 
relations  (see  Appendix  A)  specify  the  position  and 
orientation  of  the  body  coordinates  with  respect  to  an 
inertial  reference  frame  as  established  by  the  XYZ 
rotations,  and  are  expressed  in  terms  of  linear  velocities 
and  Euler  angular  rates. 

D.  HYDRODYNAMIC  COEFFICIENTS 

Although  development  of  the  hydrodynamic  coefficients  is 
not  a  thrust  of  this  thesis,  a  brief  description  of  their 
derivation  is  warranted. 

The  hydrodynamic  coefficients  provide  the  source  of  the 
behavioral  characteristics,  and  thus  the  responsiveness,  of 
a  particular  underwater  vehicle. 

These  coefficients  are  the  result  of  a  Taylor  series 
expansion,  in  which  only  the  first  order  terms  are  saved, 
based  on  the  motion  variables  of  the  hydrodynamic  forces  and 
moments.  The  hydrodynamic  coefficients  are  non- 
aimensionalized  and  can  be  considered  constants  within 
limited  operating  ranges.  [Ref.  6] 

There  are  currently  two  primary  methods  utilized  for 
obtaining  hydrodynamic  coefficients.  The  first  is  based  on 
tow  tank  experiments  using  planar  motion,  and  rotating  arm 
mechanisms.  The  second  is  a  geometric  analytical  approach 
using  semi-empirical  techniques.  [Ref.  4] 


The  coefficients  used  for  this  thesis  are  those  that 
were  determined  using  the  analytic  approach  for  an  SDV 
simulator.  [Ref.  4] 

The  coefficients  thus  selected  were  chosen  because  of 
convenience  and  availability  rather  than  any  particular 
desirability  of  the  hydrodynamic  characteristics  implied. 

E.  PROPULSION  AND  CROSSFLOW  DRAG  MODELING 

1.  Propulsion  Plant  Modeling 

In  NCSC * s  report  by  Crane,  Summey  and  Smith  [Ref. 

4],  propulsion  plant  modeling  is  discussed.  In  that  report 
they  list  the  effects  of  propulsion  on  the  motion  of  a 
submersible. 

propulsion  thrust 
propeller  slipstream  effects 
propulsive  to.  que 
propulsion  induced  hull  effects 
Of  these  four  effects  only  the  first  two  are  considered 
substantial  and  the  last  two  are  considered  negligible. 

The  propulsive  thrust  equation  was  derived  by  NCSC 
by  curve  fitting  experimental  data  and  the  propeller 
slipstream  effects  are  modeled  as  a  function  of  vehicle 
speed,  propeller  rpm,  and  geometry.  [Ref.  4] 

2.  Crossflow  Drag  Modeling 

Since  the  AUV  geometry  selected  in  this  study  is 
essentially  a  low  aspect  ratio  wing  design  and  not  a  body  of 
revolution,  its  body  cross-sections  are  nearly  rectangular 


r* 

r. 


rather  than  circular.  Because  of  this  an  integral  strip 
theory  formulation  of  crossflow  forces  and  moments  was 
developed  and  incorporated  into  the  equations  of  motion  as 
given  in  Appendix  A. 

F.  BOW  PLANE  INFLUENCE 

Bow  plane  action  serves  to  augment  stern  plane  control 
over  pitch  motions,  but  adds  to  the  hydrodynamically  induced 
drag  on  the  vehicle.  When  port  and  starboard  bow  planes  are 
separately  controlled,  active  control  over  vehicle  roll 
motion  may  fce  accomplished.  Thus  the  coefficients  relating 
to  the  heave  and  pitch  motions,  axial  drag,  and  roll  motions 
have  been  modified  here  to  allow  separate  active  roll 


control . 


Ill 
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A.  GENERAL 

The  overall  objective  of  this  chapter  is  to  fully 
describe  the  techniques  used  to  linearize  the  highly  non¬ 
linear  equations  of  motion.  A  step  by  step  and  term  by  terra 
development  of  the  linearized  equations  are  presented  and 
all  variables  are  completely  specified  in  their  relation  to 
the  AUV  in  this  study. 

A  description  of  the  linearization  point  and  the 
ramifications  of  linearization  about  a  straight  line  path  is 
also  considered  in  this  section. 

B.  LINEARIZATION  PROCEDURES 

Linearization  of  the  vehicle  dynamics  is  required  for 
the  design  of  the  vehicle  control  system.  The  linearized 
equations  also  serve  as  the  model  reference  for  the 
controller.  The  desired  form  is  the  state  space 
representation  of  the  equations  of  motion  given  as, 

M  AX  ■  h  AX  +  B  AH  (3.1) 

As  discussed  in  Chapter  II,  the  vehicle  dynamics  are 
represented  in  the  following  form: 

8  x  =  ■£(*,£, u)  (3.2) 
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where  H  is  the  mass  matrix,  x  is  the  time  derivative  of  the 
state  vector  x  and  \&  is  the  input  vector.  For  the  immediate 
purpose  at  hand,  z  may  be  considered  to  be  part  of  the  state 
vector  x*  Proper  separation  will  be  discussed  in  what 
follows . 

Linearization  is  accomplished  by  a  Taylor  series 
expansion  about  a  nominal  path  or  trajectory  given  generally 
by  (XoCb)  )  •  with  only  the  first  order  terms  being 
retained.  The  following  form  is  then  obtained: 


M  XQ  +  MAX  -  f(XofHo)  + 


3x 


3f 

—  *  — o  — 

5u 


(3.3) 


where,  if  AX  **  (X  -  X0)  /  and  AY  ■  (y  -  uQ)  ,  and  Equation 
(3.3)  becomes. 


M  AX  « 


3f(x  ,u  )Ax  3f(x  ,u_)Au 
_  —  — o  —o  —  ,  —  — o  — o  — 


9x 


9u 


(3.4) 


Defining  h  - 


91(VV 

- 5x — 


and  fi  = 


3£<2o'Ho> 

5u 


the  desired 


state  space  form  is  obtained. 


C.  APPLICATION  TO  VEHICLE  MODEL 

The  state  space  model  is  a  12  state  model  that  can  be 
separated  into  two  state  vectors  x  and  z.  The  state  vector 
X  represents  the  three  linear  velocities  and  corresponding 


\  >  ,K  «  -y 


rn  t  r-w 


three  angular  rates  about  an  orthogonal  coordinate  system 
fixed  in  the  body  as  defined  in  Equation  (2.5). 

The  state  vector  £  represents  the  six  kinematic 
relations,  three  coordinate  positions  and  three  Euler  angles 
and  is  defined  in  Equation  (2.6). 

The  two  sets  of  six  equations  that  result  are  of  the 
form: 


IT1!  (x,z,u) 

(3.5) 

• 

z  =  a  (X/i) 

(3.6) 

The  control  vector  u  is  the  input  vector  and  is  defined 
by  Equation  (2.7). 

By  combining  both  state  vectors,  the  model  state  vector 
is  defined, 

X  *  [X#£]T  =  [uvwpqrXYZ<j>9  ip]T  (3.7) 


Once  the  model  state  vector  and  control  vector  are 
defined,  the  A  matrix  and  £  matrix  must  be  determined.  The 
A  matrix  formulation  is  represented, 


and  by  similar  formulation  the  1  matrix  is, 


An  element  by  element  formulation  of  the  &  and  1 
matrices  are  complex  and  '  require  careful  attention.  The 
particular  functional  form  of  the  derivative  expressions 
can,  however,  be  obtained  analytically  and  depending  on 
whether  x0  and  £0  are  time  dependent  or  constant,  the 
analytical  derivatives  become  time  variant  or  not.  For  the 
case  of  linearization  about  a  straight  line  flight  path, 
these  derivatives  are  constant  which  makes  the  control 
computations  easier  than  for  more  complicated  nominal  flight 
conditions . 

D.  LINEARIZED  VARIABLES  ABOUT  STRAIGHT  FLIGHT  PATHS 

One  convenient  feature  concerning  the  linearization 
about  a  straight  flight  path  with  forward  speed,  uQ,  is  that 
h  and  fi  become  constant  matrices  where  the  coefficients  are 
relatively  simple  functions  of  the  forward  speed.  Also, 
since  the  nominal  path  is  associated  with  neither  rotation 
nor  cross-track  or  depth  translation,  the  incremental 
variables  Ax  and  Au  are  identical  to  the  actual  variables  x 
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and  ji  except  for  the  longitudinal  velocity  and  position. 


The  linearized  dynamics  in  the  axial  direction  become: 


3t  ^ 


x  =  u 
—  — o 


(3.10) 


so  that  as  far  as  the  linearized  system  dynamics  are 


concerned  AX(1)  ■  0  and  Ax(t)  ■  Uo(t).  While  this  feature 


is  convenient ,  it  does  not  provide  information  on  the  second 


order  effect  of  control  surface  action  slowing  down  the 


vehicle. 


A  possible  approach  to  alleviating  this  deficiency  in 


the  linear  model  could  be  to  modify  the  axial  direction 


equ  ion  of  motion  so  that  the  drag  effects  of  control 


surface  action  are  related  to  |<S2I  rather  than  <5£.  This  is 


beyond  the  scope  of  this  thesis. 


wiVtWK1'1 
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A.  GENERAL 


This  chapter  contains  a  review  of  optimal  c  Dntrol 
techniques  as  developed  and  used  in  this  study  for  the 
control  of  autonomous  underwater  vehicles.  Such  an 
autopilot  has  been  classically  treated  as  a  series  of 
interconnected  feedback  loops  for  independent  control  of 
depth  and  control  of  course  and  heading,  while  roll  control 
of  the  vehicle  has  been  left  passive.  Control  of  the  sixth 
degree  of  freedom,  longitudinal  velocity,  has  not  been 
considered  important  and  a  constant  '  thrust  or  propeller 
speed  has  been  assumed. 

While  control  of  all  six  degrees  of  freedom  may  be 
important  in  the  end  for  future  AUV  operations,  and 
particularly  in  the  transition  from  cruise  to  hover  modes, 
this  is  not  the  primary  focus  here.  Instead,  this  chapter 
deals  with  the  state  of  the  art  in  systems  concepts  for 
underwater  vehicle  course  and  depth  control,  together  with  a 
review  of  the  modern  multivariable  system  controls  methods 
used  in  modern  autopilot  design. 

B.  CLASSICAL  CONTROL  OF  COURSE  AND  DEPTH 

Simple  autopilots  have  long  been  of  interest  in 
relieving  the  human  operator  of  onerous  tasks  and  preventing 
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fatigue.  Classical  design  techniques  have  considered  depth 
and  course  control  as  separate,  non-interacting  control 
systems.  The  depth  controller  directs  commands  to  the  stern 
planes  based  on  an  error  between  pitch  angle  command  and 
vehicle  pitch  angle  where  the  pitch  command  is  proportional 
to  depth  error.  Course  heading  controllers  provide  rudder 
angle  commands  proportional  to  heading  angle  error.  Walker 
[Ref.  7]  recently  proposed  the  addition  of  a  cross  track 
position  feedback  loop  using  yaw  angle  damping  to  control 
the  cross  track  distance  for  automatic  track  control. 

Most  vehicle  controllers  in  practice  rely  on  classical 
concepts  with  protection  limits  on  command  signals  so  that 
control  surface  commands  can  be  limited  in  magnitude  and 
rate.  Adaptive  steering  controllers  have  been  proposed  as 
an  extension  for  course  maintenance  in  heavy  seas  when 
optimized  gain  settings  are  based  on  calm  weather  ship 
characteristics  [Ref.  8],  The  main  limitation  of  autopilot 
designs  based  on  classical  concepts  are, 

1.  Ship  characteristics  vary  strongly  with  speed  so  that 
gain  settings  for  all  of  the  major  loops  have  to  be 
adjusted  to  maintain  optimum  performance  under  wide 
operating  conditions. 

2.  Gains  set  based  on  maximum  actuation  limits  and 
usually  designed  to  regulate  vehicle  depth  and  course 
about  nominally  fixed  reference  settings. 

3.  Control  of  depth  and  course  changes  (i.e.,  rapid 
maneuvering)  is  not  easy  and  usually  not  automated. 

The  control  of  rapid  maneuvering  is  more  suited  to  the 
more  recent  multivariable  control  system  structures  such  as 
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those  involving  Model  Following  Controllers,  and  Model  Based 
Compensators  as  proposed  by  Milliken  [Ref.  9]. 

C.  REVIEW  OF  OPTIMAL  CONTROL  CONCEPTS  RELATING  TO 

CONTROLLER  DESIGN 

1.  LOR  Summary  and  Discussion 

Much  has  been  written  about  the  application  of 
Optimal  Control  Concepts  to  the  design  of  feedback  systems 
for  both  output  regulation  and  input  tracking.  Kwaakernaak 
and  Sivan  [Ref.  10]  present  a  discussion  of  design  methods 
based  on  state  of  the  art  to  1971.  Kaufman  and  Berry  [Ref. 
11]  have  provided  examples  of  autopilot  design  methods  based 
on  linear  optimal  regulator  (LQR)  methods  and  model 
following  techniques.  Milliken  [Ref.  9]  has  showed 
recently,  the  use  of  Model  Based  Compensators  for  providing 
multi-degrees  of  freedom  control  for  a  submarine  depth  and 
course  control  using  linear  control  techniques — similar  to 
those  used  in  this  work.  Most  recently,  non-linear  control 
methods  have  been  proposed  by  Slotine  [Ref.  12],  and  Yoeger 
and  Slotine  [Ref.  13]  to  provide  robust  trajectory  control 
for  underwater  vehicles.  Using  linear  control  procedures, 
the  vehicle,  or  object  to  be  controlled  is  described  by  a 
linear  state  variable  dynamic  model  for  response  computation 
by  equations  of  the  form, 


Xp ( ~  &p  Xp ( +  ^p  Mp ( t ) ; 


Xp ( 0)  given 


(4.1) 


in  which  the  matrices  Ap  and  Bp  represent  constant 
coefficient  terms,  and  xp(t)  and  _Up(t)  respectively, 
represent  the  vector  of  motions  (positions  and  velocities) 
and  the  control  actions  (control  surface  deflections) . 

The  design  of  a  linear  optimal  regulator  (LQR) 
control  is  based  on  the  notion  that  if  some  non-zero  initial 
condition,  x(0) ,  is  established,  then  ap(t)  can  be  designed 
so  that  the  non-zero  state  values  can  be  reduced  to  the 
equilibrium  values  x(t)  «  o,  x(t)  »  0  with  a  control 
operation  given  by, 

lip(t)  -  -  K  Xp(t)  (4.2) 

where  £  is  found  from  the  minimization  of  the  quadratic 
performance  index, 

00 

J  ■  /  (XT  fi  X  +  liT  fi  li)  dt  (4.3) 

0 

Here,  Q  is  a  non-negative  definite  square  symmetric 
weighting  matrix  for  response  magnitudes  and  £  is  a  positive 
definite  square  symmetric  weighting  ,  matrix  for  control 
effort.  Q.  is  size  nxn,  and  £  has  rank  equal  to  the  number 
of  control  inputs  modeled  (r) . 

The  solution  for  £  becomes  a  matrix  of  size  rxn 
found  as  the  solution  to, 


£  -  B"1  fiT  £ 


(4.4) 


where 


£  h  +  At£  +  £  -  £(£ ”1£"1ET)  £  -  0  (4.5) 


The  eigenvalues  of  the  closed  loop  regulator  are 
determined  from  the  combined  state  and  co-state  system 
equations.  They  are  given  by  the  eigenvalues  of  the 
composite  matrix  ££  where, 


SB 


&  £  E”1  £t 

-A  -BT 


(4.6) 


It  can  be  show  [Ref.  10]  that  £  is  also  given  by, 


£  -  CH2l  [Hi]”1 


(4.7) 


where  Hi#  H2  are  the  nxn  partitions  of  the  matrix,  W, 


H 


Hi 

H2 


(4.8) 


formed  from  columns  of  stable  eigenvectors  of  SS.  It  has 
also  been  found  that  the  use  of  real  part  and  imaginary 
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parts  of  a  complex  conjugate  eigenvector  as  adjacent  columns 
of  H  where  a  complex  pole  pair  exist,  eliminates  the  need 
for  complex  matrix  inversion  [Ref.  10]. 

The  design  by  minimization  of  J  in  Equation  (4.3) 
yields  the  closed  loop  control  system  equations, 

ip(t)  -  (A  -  B  K)Xp(t);  Up  -  -JK  xp;  xp(0)  given  (4.^) 

where  the  steady  state  response  is  zero  for  both  xp  and  up. 

The  state  vector  may,  in  many  cases,  be  considered 
as  a  deviation  vector  from  a  desired  constant  level,  and  it 
is  quite  appropriate  for  the  steady  state  values  of  xp  and 
up  to  go  to  zero.  However,  in  the  reality  of  seme  cases, 
the  maintenance  of  a  constant  level  in  some  elements  of  the 
state  vector  requires  a  non-zero  steady  state  control  signal 
level  and  in  these  cases  iip(°°)  =0.  If  a  steady  state  level 
must  be  maintained  for  any  element  of  the  state  vector,  the 
steady  state  equations  are  first  solved  as, 

0  -  &p  Xp(°°)  +  Bp  lip(°°)  (4.10) 

Equation  (4.10),  subtracted  from  Equation  (4.1) 
reduces  these  cases  to  the  equivalent  of  a  regulator  control 
problem  by  shift  of  variable, 

Xp (t)  58  &p(Xp(t)  —  xp(co))  +  Bp ( — p ( t )  “  Up (°° ) )  (4.11) 

28 


where  the  new  variables 


Xp(t)  -  xp(«) 


(4.12) 


and 


Up(t)  "  Up  (co) 


(4.13) 


are  related  in  a  control  law 

Mp(t)  ~  %(”)  =  -K(Xp(t)  -  Xp(oo))  (4.14) 


or 


%(t)  =  Up(<»)  -  K(%(t)  -  Xp(oo))  (4.15) 

The  above  discussion  has  been  limited  to 
deterministic  signals  and  to  the  assumption  '  that  all 
physical  state  variables  are  either  measurable  or  determined 
in  a  full  state  observer  [Ref.  10]. 

Where  the  output  of  the  controlled  process  is  to  be 
regulated,  the  above  techniques  may  be.  used  to  design  the 
slements  of  '  \e  feedback  gain  matrix  thus  avoiding  the 
complex  task  c  designing  separate  control  loops  from  each 
variable  in  the  process.  The  method  is  powerful,  but 
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requires  skill  in  the  selection  of  appropriate  £  and  £ 
factors . 


2.  Tracking  Control  Systems— (LMFC  +  MRAC) 

Where  the  control  system  is  required  to  drive  a 
process  so  that  the  output  tracks  an  input  variable  within 
acceptable  error  bounds,  the  problem  is  further  compounded. 
Sven  more  difficult  is  to  achieve  the  tracking  of  several 
simultaneous  inputs  by  the  various  outputs  of  the  driven 
process.  During  the  late  1960's  and  early  1970's,  much 
attention  was  placed  on  linear  model  following  controls 
(LMFC)  and  model  reference  adaptive  controls  (MRAC)  to 
provide  the  acceptable  tracking  behavior  of  multivariable 
systems.  Kaufman  and  Berry  [Ref.  11]  described  the 
application  to  flight  control,  and  Landau  [Refs.  14,15]  gave 
a  survey  of  design  techniques  and  system  structures  in  which 
it  became  clear  that  a  model  of  the  system  to  be  controlled 
was  needed  to  represent  the  desired  time  behavior  of  the 
system  state  variables.  The  system  control  variables  then 
became  a  function  of  the  input  variables  to  the  model,  and 
the  model  state  variables,  in  addition  to  the  feedback  of 
system  state  variables.  Thus  better  information  than  could 
be  derived  by  feedback  was  used  to  drive  outputs  to  track 
inputs . 

The  use  cf  MRAC  techniques  allows  for  not  only  model 
following,  but  also  the  provision  of  adapting  gains,  or 
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model  parameters  in  achieving  precise  control  when  system 
operating  conditions  change. 

One  of  the  difficulties  pointed  out  by  Landau  [Ref. 
14],  is  that  controller  parameters  need  to  change  when  he 
plant  operating  conditions  changed.  Thus  using  a  reference 
model  not  only  provides  the  robustness  achieved  by 
predictive  and  corrective  control  but  also  provides  the 
opportunity  to  update  model  and  control  parameters 
automatically. 

Restricting  the  discussion  to  Linear  Model  Following 
Controls  (LMFC) ,  the  control  issues  are  analyzed  as  follows: 
the  plant  model  is  given  by, 


Xp  -  &p  5?p  +  Ip  Bp 

(4.16) 

• 

¥p  =  £p  2£p 

(4.17) 

and  a 

suitable  model  of  the  plant,  but  with 

•  desirable 

dynamic 

response  characteristics  (response  time. 

stability, 

etc.)  is  given  by. 

• 

.^m  =  ^m  ^m  +  Sm  (t) 

(4.18) 

• 

am  -  0 

(4.19) 

¥m  =  £m  Xm 

(4.20) 
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then  the  control  signals,  Up,  which  minimize  the  weighted 
integral  of  errors  between  model  and  plant  are  given  by, 

Up  =  El  lljn  +  E 2  £m  E3  2£p  (4.21) 

where  the  errors  are  defined  as, 

e  -  £m  “  *p  (4.22) 

and  the  performance  index  minimized  is, 

00 

J  =  /  (eT  Q  £  +  UpT  £  ap)  dt  (4.23) 

and  Q,  and  fi  are  weighting  matrices  as  discussed  earlier. 

The  computation  of  the  gain  matrices,  £lf  £2/  and  £3 
are  fortunately  made  easier  by  considering  the  combined 
system,  model  plus  plant  as  a  coupled  linear  system.  Also, 
to  overcome  problems  that  ax'ise  when  the  signals  to  be 
tracked,  ym(t)  are  derived  from  inputs,  iim(t)  ,  that  are  not 
impulses,  it  is  convenient  to  consider  that  the  additional 
model  equations, 

•  . 

Um  =  0  (4.24) 

be  incorporated  together  with  um  as  a  composite  state 
vector, 
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(4.25) 


XT  *  [XpT,XmT,llmT3 
to  get  the  system  equations, 

Xp  hp  0  0  2Sp  £p 

“  0  &m  Em  +  0  ^pj  (4.26) 

Urn  0  0  0  um  0 

mm 

Now,  application  of  the  LQR  technique  to  the  composite 
system  given  above  yields, 

Mp  =  -CE”1  BpT][£][xp  iim3T  (4-27) 

where  £  now  is  of  dimension  (np  +  nm  +  rm) ,  and  [B-1  fiT  P] 
is  partitioned  in  three  parts, 

CE_1  Bt  E]  =  [K3  K2  El]  (4.28) 

By  varying  the  weighting  factors  within  the  matrix, 
Q,  selected  errors  may  be  penalized  more  heavily  than  others 
in  the  optimal  control  trade-off.  Also,  selection  of 
parameters  within  B  may  be  used  to  provide  a  trade-off 
between  a  sluggish  or  sensitive  control  design.  Details  of 
numerical  values  used  in  the  design  of  the  autopilot 
controls  are  given  later  in  Chapter  VI. 
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3.  Near  Time  Optimal  Maneuvering  Models 


The  use  of  the  system  structure  implied  by  Equation 
(4.26)  and  the  resulting  control  law.  Equation  (4.21),  is 
particularly  useful  when  considering  near  time  optimal 
positioning  of  inertial  objects.  It  is  well  known  that  time 
optimal  position  control  of  a  massive  object  requires  a 
bang-bang  application  of  force  or  torque.  These  concepts 
are  recently  being  considered  in  robot  tracking  control 
[Ref.  16],  and  the  sliding  control  described  in  [Ref.  17]. 
So  also,  in  the  field  of  LMFC  for  underwater  vehicle 
maneuvering  control,  it  is  expected  that  rapid  maneuvering 
will  require  some  form  of  bang-bang  operation  of  control 
surfaces.  Bang-bang  operation,  in  principle,  is  simple, 
consisting  of  a  sequence  of  stepwise  control  actions,  yet 
knowledge  of  switching  times  for  anything  other  than  very 
low  order  systems  make  the  principle  difficult  to  implement. 
The  outcome  of  the  above  discussion  then  leads  to  the 
development  of  vehicle  maneuvering  models  based  oh  use  of  a 
series  of  constant  setting  for  control  surfaces  that  make  Up 
the  model  input  vector  jjm.  At  times  during  the  response  of 
the  model  where  switching  should  occur,  the  control  surfaces 
change  setting  rapidly  as  if  by  imposition  of  an  impulse 
command.  Therefore,  if  it  is  considered  that  surface 
settings  change  levels  at  discrete  but  arbitrary  times,  the 
unforced  nature  of  the  model  reference  states,  in  Equation 


(4.26)  are  preserved  and  the  application  of  the  LMFC  system 
is  valid. 

For  every  reflexive  maneuver  envisioned  during  the 
operation  of  an  AUV  life,  it  is  foreseen  that  maneuvering 
logic  can  be  developed  on  an  algorithmic  basis  to  determine 
switching  times,  using  logic  to  be  developed  and  the 
fil/  K2  an<*  K3  matrices  as  shown  in  Figure  4.1.  These  data 
can  be  stored  inside  on-board  processors  so  that  on  command 
from  the  high  level  controller  or  expert  system,  new 
computations  for  jjp  can  be  implemented  immediately. 

The  development  of  a  maneuvering  logic  as  a  command 
generation  system  for  a  dive  maneuver  will  be  discussed  in 
more  detail . 


Figure  4.1  Block  Diagram  for  a  Model  Following  Autopilot 


V.  SIMULATION  TECHNIQUES 


A.  GENERAL 

This  chapter  contains  a  discussion  of  the  computational 
program  structures  used  in  this  study.  All  simulations  were 
performed  using  the  Dynamic  Simulation  Language  (DSL)  [Ref. 
5]  code  for  the  simulation  of  linear  and  non-linear  system 
response  as  a  function  of  time.  Internal  numerical 
integration  routines  make  this  aspect  of  the  solution 
transparent  to  the  user.  The  user  provides  only  the  details 
of  the  particular  equations  employed.  In  this  work,  DSL  was 
used  for  the  simulation  of  both  uncontrolled  and  autopilot 
controlled  vehicle  responses.  However,  as  part  of  the 
design  procedure  for  the  autopilot,  the  complete  set  of 
feedforward  and  feedback  gains  were  established  using  ETAT — 
a  specially  developed  program  for  the  computation  of  linear 
optimal  control  gains.  The  pertinent  linkages  between  DSL 
and  ETAT  were  developed  and  implemented  during  this  study. 
More  detailed  descriptions  follow. 

B.  COMPUTATION  OF  FEEDFORWARD  AND  FEEDBACK  GAINS 

While  the  theory  behind  the  need  for  feedforward  gains 
for  optimal  model  following  autopilots  has  been  given  in 
Chapter  IV,  this  section  discusses  the  program  organization 
used  in  their  computation. 
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The  outline  organization  of  program  ETAT  is  shown  in 
Figure  5.1.  ETAT  reads  and  writes  values  of  and  fifi,  as 
computed  within  the  framework  of  the  DSL  simulation  and  also 
reads  user  input  values  for  the  tracking  error  weighting 
matrix,  Q,  and  the  control  input  weighting  matrix,  E* 
Particular  values  used  for  Q,  and  £,  are  given  later  in 
Chapter  VI. 

Subroutine  MTXEXP  computes  the  matrix  exponential 
associated  with  &&,  and  the  discrete  time  input  matrix 
associated  with  &&  and  but  this  section  has  not  been 
used  here. 

Subroutine  ROOTS  is  used  for  the  computation  of  both 
eigenvalues  and  eigenvectors  of  a  square  matrix  (&&) ,  and 
calls  the  IMSL  double  precision  library  routine  EIGRF  and 
its  associated  subroutines. 

OPTIMA  is  the  subroutine  used  for  assembly  of  the 
composite  state  and  co-state  matrix,  ££.  OPTIMA  also  calls 
EIGRF  and  computes  the  closed  loop  system  eigenvalues  and 
vectors.  These,  as  given  earlier  in  Chapter  IV,  are  used  to 
form  the  solution  of  the  matrix  Ricatti  equation  and  the 
overall  matrix  of  gains,  i.e..  Equation  (4.4).  Partitions 
of  the  overall  gain  matrix  give  the  individual  matrices, 

£2  and  £3  in  Equation  (4.28). 

A  listing  of  the  major  subroutines  used  in  program  ETAT 
are  provided  in  the  appendix  for  the  interested  reader, 
although  use  of  ETAT  without  proper  linkages  to  DSL  and  the 
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appropriate  IMSL  double  precision  library  would  not  be 
proper. 

C.  REFERENCE  MODEL  DEVELOPMENT 

As  discussed  earlier,  the  reference  model  is  a  full  12 
state  representation  of  the  AUV.  The  reference  model  can  be 
represented  by: 

*m  "  AA  Xm  +  ££  lim  lim  -  0  (5.1) 

A  computational  problem  in  subroutine  OPTIMA  can  arise 
because  of  the  multiple  zero  eigenvalues  associated  with 
several  of  the  modes  in  the  above  equations.  This  problem 
has  been  overcome  here  by  inserting  very  small  values, 
"(Mi»  on  the  fc®Y  diagonal  elements  of  the  AA  matrix  so  that 
distinct  eigenvalues  result.  Since  the  (X)  values  are 
extremely  small,  their  effect  on  the  system  poles  is 
negligible  and  the  problem  of  multiple  repeated  poles  is 
eliminated. 

It  is  conceivable  to  have  a  series  of  reference  models, 
one  for  each  of  several  reflexive  maneuvers.  Each  maneuver 
will  have  its  associated  logic  that  will  generate  the 
control  input  to  the  model  and  thus  provide  the  model 
reference  states. 

For  this  thesis,  only  one  such  maneuver,  a  dive 

4  * 

maneuver,  was  investigated.  Logic  for  the  dive  maneuver  is 
based  on  an  application  of  bang-bang  optimal  control  theory, 


thereby  yielding  tine  optimal  response.  The  methodology 
here  is  to  deflect  stern  planes  up  and  the  bow  planes  down 
to  initiate  a  pitch  rate  (p)  until  the  vehicle  achieves  some 
predetermined  pitch  angle  (e )  •  For  what  is  considered 
reflexive,  or  emergency  obstacle  avoidance,  a  large  angle  is 
desired.  Assuming  that  the  submersible  is  directionally 
stable,  some  small  stern  plane  angle  must  then  be  maintained 
to  keep  a  constant  pitch  angle,  dependent  on  speed,  until 
such  time  when  the  control  action  should  provide  an  opposite 
effect  to  come  out  of  the  dive  and  steady  at  a  new  depth. 
An  example  of  this  control  action  is  shown  in  Figure  5.2. 

Given  limits  on  control  surface  deflection  and  maximum 
pitch  angle  during  the  dive,  this  type  of  control  action 
should  provide  an  optimal  response  for  a  change  in  depth. 

With  this  control  logic  preprogrammed  into  an  AUV, 
whenever  the  supervisory  control  system  calls  for  a  dive 
maneuver,  the  logic  can  provide  the  control  input  for  the 
reference  model  and  thus  an  optimal  path  can  be  created 
quickly;  one  that  the  controller  can  track  and  vehicle  can 
follow. 

The  logic  for  the  dive  maneuver  is  crude,  however,  this 
is  a  trade-off  for  ease  in  programming  the  algorithm  used 
for  the  dive  maneuver.  When  programming  one  of  these 
reflexive  maneuvers  one  must  be  cautious  not  to  program  a 
maneuver  that  is  beyond  the  capability  of  the  vehicle. 


a 


A  conceptual  objective  is  to  have  many  maneuver 

* 

algorithms  preprogrammed  into  a  vehicle  into  a  "bag"  of 
maneuvers.  •  This  bag  of  maneuvers  would  be  at  the  disposal 
of  the  supervisory  control.  This  supervisory  control  would 
be  the  manager  of  the  bag  of  maneuvers,  as  earlier  indicated 
in  Figure  1.3,  and  would  receive  its  instructions  from  the 
on-board  expert  system  or,  in  the  future,  artificial 
intelligence. 

For  the  many  types  of  standard  and  emergency  situations 
required,  collision  or  obstacle  avoidance,  a  proper  maneuver 
can  be  chosen  and  executed  quickly  and  efficiently  without 
excessive  computational  burdens  that  would  otherwise  lead  to 
a  tardy  response. 

D.  SYSTEM  SIMULATION  METHOD 

1.  Dynamic  Simulation  Language  (DSL) 

DSL  is  a  Fortran  based  simulation  language  for 
digital  simulation  of  continuous  systems.  DSL  uses  a 
building  -block  approach  to  programming.  Programs  can  be 
very  simple  or  they  can  become  extremely  complex  when  all 
the  functions  of  DSL  are  utilized.  The  user  can  enter 
Fortran  statements  in  any  order  and  DSL  can  sort  and  solve 
these  equations  effectively.  The  user  can  also  include 
fortran  subroutines  and  use  the  expansive  I/O  facility  of 
DSL.  One  other  key  feature  is  the  integration  routine 
capability.  The  user  has  the  choice  of  nine  integration 
methods;  four  fixed-step,  two  variable-step  and  three 
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variable-step,  variable  order  methods.  DSL  was  chosen 
primarily  because  it  easily  can  solve  differential  equations 
and  it  contains  many  internal  functions  that  normally  would 
have  to  be  programmed  by  the  user. 

DSL  has  four  phases  of  program  execution; 

TRANSLATION 

COMPILATION 

SIMULATION 

GRAPHICS 

DSL  translates  all  the  DSL  code  into  Fortran 
statements.  Once  the  code  is  translated,  it  is  then  linked 
to  the  VS  compiler  and  the  code  is  compiled  and  stored  as  an 
executable  file.  Upon  completion  of  the  compilation  phase, 
the  simulation  phase  begins,  and  the  system  clock  starts, 
and  simulation  continues  until  the  system  reaches  its  user 
specified  finish  time.  The  last  phase  of  problem  execution 
includes  the  graphic  capability  of  DSL.  Saved  output  data 
can  be  plotted  or  graphed  using  the  graphics  post-processor 
and  the  specific  hardware  supported. 

2.  Problem  Simulation 

As  mentioned  earlier,  DSL  uses  a  building-block 
approach  to  programming.  The  major  blocks  and  general  flow 
of  program  simulation  are  shown  in  Figures  5.3  and  5.4.  To 
fully  understand  the  simulation,  and  the  controller  design, 
and  control  action,  a  detailed  breakdown  and  discussion  is 
required. 


CALCULATE 
M  MATRIX 


INITIALIZE 

MATRICES/ARRAYS 


The  first  section  of  the  simulation  is  the  CONSTANT 
block.  In  this  block  all  of  the  hydrodynamic  coefficients 
and  vehicle  constants  are  read  into  the  program. 

The  second  section  is  the  INITIAL  block.  In  this 
section,  all  of  the  calculations  not  part  of  the  integration 
routines  and  those  needed  in  establishing  parameters  and 
initial  conditions  are  performed.  This  is  also  where  all 
variables  are  initialized.  The  following  calculations  occur 
in  this  section: 

1.  All  matrices  and  arrays  are  initialized  to  zero. 

2.  The  length  and  weight  fractions  for  a  four  term  gauss 
quadrature  are  initialized. 

3.  The  breadth  and  height  terms  are  read  in.  These  terms 
will  be  used  in  the  gauss  quadrature  integration  for 
the  crossflow  drag  terms, 

4.  The  thrust  is  then  calculated  for  the  propulsion 
model . 

5.  The  non-zero  elements  of  mass  matrix  M  are  calculated. 

6.  The  square  mass  matrix  &  has  rank  of  six  is  then 
inverted  using  the  IMSL  routine  LINV2F. 

7.  The  non-zero  elements  of  the  &  matrix  are  calculated. 
These  elements  are  the  coefficients  of  the  first  order 
terms  in  the  Taylor  series  expansion  about  a  specific 
operating  point. 

8.  The  non-zero  elements  of  the  B  matrix  are  then 
calculated. 

9.  The  next  step  is  to  multiply  the  inverse  of  the  mass 
matrix  to  obtain  the  coefficients  of  the  state 
equation,  (5.1). 

10.  The  last  task  for  the  initial  section  is  to  read  in 
the  computed  feedforward  and  feedback  gains  from  the 
program  ETAT  that  are  to  be  used  in  the  autopilot 
control  law. 
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The  third  block  of  the  main  program  is  called  the 
DERIVATIVE  section.  Here,  all  the  first  order  equations 
that  must  be  integrated  and  solved  are  assembled.  The 
DERIVATIVE  section  of  this  simulation  is  comprised  of  three 
major  subsections,  one  for  each  major  section  in  the 
simulation. 

1.  Linear  reference  model  providing  command  generation. 

2.  Control  law  linking  model  and  vehicle  response  to 
control  surface  actions. 

3.  Nonlinear  model  for  simulating  vehicle  response. 

The  control  vector  um  is  the  input  to  the  linear 
model,  generated  from  the  maneuver  logic  contained  within 
the  DYNAMIC  section.  This  section  will  be  discussed  later. 

Once  the  control  input  is  established,  the 
derivative  expressions  of  the  linear  reference  model  are 
formulated  in  terms  of  the  matrices  AAm  and  BB^ 

After  the  linear  model  derivatives  are  established, 
the  model  states  2cm  and  model  inputs  um  are  passed  to  the 
Control  Law.  The  Control  Lav?  (Equation  (4.21))  represents, 
in  software,  the  gains  that  would  be  incorporated  in  the 
vehicle. 

The  input  vector  Up  represents  the  inputs  to  the 
actual  vehicle,  in  this  case,  defined  by  Equations  (3.2). 

The  derivatives  of  the  vehicle  state  variables  are 
formed  as  the  last  part  of  the  DERIVATIVE  section  in 
preparation  for  numerical  integration  using  the  fifth  order 
variable  step  Runge-Kutta  technique. 


The  model  states  and  inputs,  as  well  as  the  vehicle 
states  and  inputs,  are  saved  for  graphical  representation 
and  data  output. 

The  fourth  major  block  of  the  simulation  is  the 
DYNAMIC  section.  In  the  DYNAMIC  section,  the  maneuver  logic 
is  programmed.  This  section  is  reserved  for  those 
computations  that  depend  on  time  and  are  independent  of  the 
system  responses.  However,  response  dependent  functions  may 
also  be  included  here  as  is  the  case  with  the  establishment 
of  the  reference  control  commands  generated  by  the  maneuver 
logic. 

The  fifth  section  of  the  program  is  the  CONTROL 
section.  Before  the  command  or  input  is  sent  to  the 
derivative  section,  the  system  time  clock  is  checked,  and  if 
"finish  time"  (f intime)  in  the  CONTROL  section  is  reached 
the  program  stops.  If  not,  the  system  increments  itself  one 
time  step  and  continues  with  the  simulation. 

Upon  completion  of  the  simulation  a  time  history  of 
all  desired  parameters  and  variables  are  saved  in  a  data 
file.  Plots  and  graphical  output  may  then  be  generated. 

3 .  Procedure  Used 

To  perform  a  simulated  run  with  a  particular 
autopilot  design  and  vehicle  speed,  an  initial  run  with  DSL 
was  required  to  establish  values  for  the  AA,  and  BB 
matrices.  These  values  were  written  on  as  output  files 
(file  FtlOFOOl  for  AA  and  file  Ft09F001  for  BB) ♦  By 


separate  run,  program  ETAT  was  used  to  read  M  and  BB,  and 
its  own  input  file  Ft05F001  for  Q  and  E  and  to  provide 
values  for  control  gains  &lt  £2  and  £3-  The  gain  matrices, 
written  on  file  Ft02F001  were  then  read  by  a  final  run  using 
DSL  for  the  controlled  vehicle  response  simulation  and 
results  were  provided  on  data  file  OUTP. 
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A.  GENERAL 

This  chapter  describes  the  efforts  and  results  of  the 
design  of  a  model  following  autopilot  for  an  AUV.  The 
controller  designed  is  only  a  partial  solution  to  the 
complete  control  over  the  six  degrees  of  freedom  of  an  AUV. 
However,  the  methodology  developed  in  this  study  could  be 
applied  to  design  a  full  30  state  controller,  12  plant 
states,  12  model  states,  and  6  control  states.  The 
controller  designed  in  this  study  is  a  19  state  controller 
using  12  plant  states,  4  model  states  relating  to  the  pitch 
plane,  wm,  qm,  zm,  em  and  the  three  control  inputs  for  this 
plane,  port  and  starboard  bow  plane  angle  and  stern  plane 
angle . 

In  Section  D,  the  base-line  controller  is  tested  and  the 
results  show  excellent  depth  control  with  excellent  time 
history  tracking.  However,  the  control  over  pitch  angle 
appeared  loose  and  in  Sections  E  and  F  attempts  were  made  to 
gain  tighter  pitch  control. 

A  test  of  controller  robustness  is  its  ability  to 
operate  over  a  range  of  vehicle  speeds  and  changing 
parameters.  In  Section  G  the  controller  was  tested  at 
speeds  of  3,  12  and  30  ft/sec,  approximately  1.8,  7  and  17 


knots,  respectively,  while  baseline  runs  were  at  6  ft/sec 
(3.f  knots) . 

Included  in  this  chapter  is  a  discussion  of  the  gains 
used,  how  they  were  derived  and  the  effects  on  the  gains  by 
varying  the  control  weight  matrix  £  and  the  control  error 
weight  matrix  Q. 

B.  RESULTS  OF  UNCONTROLLED  MANEUVERING 

The  first  simulation  runs  that  were  made  early  in  this 
study  were  to  test  the  non-linear  model.  One  maneuver  that 
was  first  tasted  was  a  turning  maneuver.  Because  of  this 
AUV's  particular  geometry  (low  aspect  wing  vice  body  of 
revolution) ,  some  unique  behavioral  characteristics  are 
displayed  as  shown  in  Figures  6.1  and  6.2,  not  common  to 
other  forms  of  underwater  vehicles.  Of  the  most  significant 
is  when  a  rudder  command  is  given  the  vehicle  rolls  out  of 
the  turn.  While  this  is  not  uncommon  for  vehicles  without  a 
sail  area,  it  is  uncommon  for  a  vehicle  with  a  cruciform 
type  stem  to  dive  on  a  turn  while  the  vehicle  rolls  out. 
Although  this  vehicle's  dynamics  are  not  representative  of 
those  common  to  submersibles,  the  behavior  has  been  verified 
experimentally.  The  purpose  of  selection  was  based  purely 
on  the  availability  and  thoroughness  to  which  the  vehicle 
dynamics  were  modeled,  and  that  program  validation  was 
easily  accomplished  from  the  data  available. 
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C.  DIVE  PLANE  MANEUVER  AND  PREDICTOR  CONTROL 

Once  the  non-linear  model  was  validated  the  controller 
design  process  then  began.  The  first  simulation  of  this 
process  consisted  of  only  predictor  control,  no  feedback  was 
incorporated.  The  inputs  generated  by  the  dive  maneuver 
logic  for  stern  and  bow  plane  input  to  the  linear  model  were 
also  fed  into  the  non-linear  vehicle  dynamics. 

This  run.  Figures  6.3  to  6.7,  provided  insight  on  the 
accuracy  of  the  linearized  version  of  the  equations  of 
motion.  Figure  6.7  shows  excellent  pitch  correlation 
between  the  model  and  vehicle. 

Figures  6.4  and  6.5  both  show  that  the  vehicle  never 
reaches  its  ordered  depth  of  100  ft.  because  the  vehicle 
equations  were  linearized  about  a  straight  line  trajectory 
at  a  constant  speed,  the  linear  model  does  not  generate  any 
speed  loss  and  subsequently  the  AUV  lags  behind  the  linear 
model,  a  result  that  was  clearly  expected.  The 
responsiveness  of  the  vehicle  is  interesting  considering  the 
slow  speed  of  3.5  knots. 

Examination  of  the  maneuver  shows  that  a  limit  of  about 
0.25  radians  and  0.18  radians,  respectively,  was  set  by  the 
command  generation  logic  while  the  maximum  pitch  angle  of  40 
degrees  was  reached  and  maintained  after  16  seconds.  Also, 
while  the  vehicle  pitch  angle  is  returned  to  a  small  value, 
when  the  control  surfaces  are  returned  to  zero,  a  small 
residual  pitch  angle  is  left.  This  is  a  small  point  that 
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Dive  Plane  Maneuver  and  Predictor  Control  Stern  Plane  Angle — 6  Ft/Sec 


Figure  6.4  Dive  Plane  Maneuver  and  Predictor  Control  Dive  Profile — 6  Ft/Sec 


Figure  5.5  Dive  Plane  Maneuver  and  Predictor 
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Figure  6.7  Dive  Plane  Maneuver  and  Predictor  Control  Pitch  Angle — 6  Ft/Sec 


could  be  corrected  by  a  refinement  of  the  command  generation 
logic. 

What  is  of  interest  is  the  magnitude  of  the  final  depth 
error  generated  by  the  difference  between  linear  and  non¬ 
linear  models.  While  the  command  generation  logic  drives 
the  linear  reference  model  to  the  targeted  depth  of  100  ft. , 
the  speed  reduction  in  the  AUV  only  provides  a  dive  to  37 
feet — clearly  indicating  the  need  for  corrective  control 
action. 

D.  EFFECT  OF  AUTOPILOT  CONTROL — BASELINE  CASE 

Figures  6.8  to  6.12  clearly  snow  the  difference  the 
controller  makes  in  attaining  the  ordered  depth.  This  was 
the  first  simulation  run  using  the  full  19  state  controller 
for  control  in  the  heave/pitch  plane.  Although  excellent 
depth  control  was  achieved,  the  pitch  control  was  considered 
a  little  loose  resulting  mainly  from  the  mismatch  between 
model  and  AUV  speed.  Figure  6.12  shows  the  overshoot  of  the 
vehicle  pitch  during  the  maneuver.  The  overshoot  of  the 
pitch  also  is  the  primary  reason  for  the  vehicle  attaining 
ordered  depth  much  more  rapidly  than  the  model  as  shown  in 
Figure  6.9. 

Other  observations  include,  the  majority  of  the  control 
action  comes  from  the  stern  plane  which  worked  much  harder 
than  the  bow  planes.  Figures  6.8  and  6.11  show  the 
di.  ices  in  control  actions  between  the  model  and  vehicle 
for  stern  and  bow  planes,  respectively. 
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Figure  6.8  Autopilot  Control — Baseline  Stern  Plane  Angle — 6  Ft/Sec 


Figure  6.11  Autopilot  Control — Baseline  Bow  Plane  Angle 


Because  of  the  disparity  in  control  efforts  an  attempt 
to  equalize  the  relative  amount  of  control  actions  and  more 
closely  following  the  model  was  made.  As  discussed  in 
Chapter  IV,  the  control  weight  matrix  B  (Table  l)  was 
initially  set  up  to  penalize  the  rudder,  rpm  and  buoyancy 
inputs,  so  that  the  primary  control  actions  would  be  from 
the  bow  and  stern  planes  as  it  would  be  for  a  dive  maneuver. 
In  this  first  run  the  weights  were  equal  and  the  resulting 
control  gains  (Table  3)  for  the  stern  plane  were  much  higher 
than  for  the  bow  planes.  An  attempt  was  made  at  sharing  the 
control  actions  where  weights  of  the  B  matrix  were  adjusted 
to  penalize  the  stern  plane  and  put  more  control  effort  in 
the  bow  planes.  This  resulted  in  a  significant  loss  in  the 
stern  plane  gain  much  less  that  one  and  only  a  very  slight 
rise  in  the  bow  plane  gain.  Although  the  resulting 
simulation  showed  more  bow  plane  action  it  did  not  follow 
the  model  well  and  the  stern  plane  became  more  active  by  the 
feedback  action.  This  increased  activity  in  the  bow  and 
stern  planes  resulted  in  very  significant  speed  loss  and 
excessive  plane  use  was  considered  unacceptable. 

Upon  further  study  of  the  vehicle  and  its  dynamics,  the 
reason  for  the  inconsistency  in  control  actions  is  that  the 
model  maneuver  treats  bow  and  stern' planes  almost  equally  in 
their  effect  but  in  fact  the  force  and  moment  generated  by 
the  stern  plane  is  an  order  of  magnitude  more  significant 
than  that  of  the  bow  planes.  Therefore,  in  future  maneuver 
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TABLE  1 


TABLE  OF  Q  WEIGHTS — BASELINE 


Pitch  Rate  Error 

Q(5,5) 

Q((5,14) 

Q(14,5) 

Q(14,14) 

Pitch  Anglfi  -Erxcrg 
Q(ll#ll) 
Q(H,16) 
0(16,11) 
0(16,16) 

Heave  Rate  Error 
0(3,3) 

0(3,13) 

0(13,3) 

0(13,13) 

Heave  Positional  Error 
0(9,9) 

0(9,15) 

0(15,9) 

0(15,15) 


0.6 

-0.6 

-0.6 

0.6 


2.5 


-2.5 

-2.5 


2.5 


1.0 

-1.0 

-1.0 

1.0 


60.0 

-60.0 

-60.0 

60.0 


TABLE  2 


TABLE  OF  £  WEIGHTS — BASELINE 


Rudder 

R(l,l) 

1.0 

x  104 

Starboard  Bow  Plane 

R(2,2) 

1.0 

X  103 

Port  Bow  Plane 

R(3  /  3) 

1.0 

x  103 

Stern  Plane 

R(4 / 4) 

1.0 

X  103 

RPM 

R(5,5) 

1.0 

x  106 

Buoyancy 

R(6,6) 

1.0 

X  106 
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CONTROL  GAINS 


O.OOOOOOOOOOE+OO 
0 . 4954081 95 IE-08 
O.OOOOOOOOOOE+OO 
0 . 4644012840E-09 
0 .259 5669 042E-05 
-0 . 4438610978E-04 
-0.5901765458E-07 
O.OOOOOOOOOOE+OO 
0.7605860458E-04 
O.OOOOOOOOOOE+OO 
0.7216705136E-05 
0 .445487 16 33E-01 
-0 . 6 9528 59507 E+ 00 
-0 .9987243305E-02 
O.OOOOOOOOOOE+OO 
0 .7628508878 E-04 
O.OOOOOOOOOOE+OO 
0. 7236512897 E-05 
0 . 4459 Q6 1566 E-01 
*0 . 697 085451 5E+00 
-0.9759326349E-02 
O.OOOOOOOOOOE+OO 
-0.6689594082E-03 
O.OOOOOOOOOOE+OO 
-0.5682397825E-04 
-0.6106134440E-01 
0 . 5192655967E+01 
-0.1004805424E+01 
O.OOOOOOOOOOE+OO 
-0.4904055922E-10 
O.OOOOOOOOOOE+OO 
-0 . 435577 02 03E-11 
-0 . 1 39306 2766 E- 07 
0.407 036 138 9 E-06 
-0.4242383239E-07 
O.OOOOOOOOOOE+OO 
0.8911601991 E-10 
O.OOOOOOOOOOE+OO 
0 . 11286 10385E-10 
0 . 192977 9261 E-06 
-0 . 1 207 29507 5E-05 
-0.4768694409F-06 


0 .72589 32550E-10 
0 . 3019079092E-04 
O.OOOOOOOOOOE+OO 
0 .4422721241 E-04 
-0 . 302507 927 SE- 04 
-0 . 116 5944258 E-05 

0.11146 19576 E-05 
0 . 466889 14 58 E+ 00 
O.OOOOOOOOOOE+OO 
0.6929793740E+00 
-0.46 767 30097 E+ 00 
-0 . 1746945879E-01 

0 . 1117 98436 4E- 05 
0 .4682061 530 E+ 00 
O.OOOOOOOOOOE+OO 
0 .6947686 167 E+00 
-0 . 4689957818E+00 
-0 . 1752998 162E-01 

-0 . 98 57 06 8 4 18 E-05 
-0 . 3859172641E+01 
O.OOOOOOOOOOE+OO 
-0.5158026393E+01 
0 . 3880347969E+01 
0.18 56 029149 E+00 

-0. 72107 98 044E-12 
-0 . 289978 06 44 E-06 
O.OOOOOOOOOOE+OO 
-0.4049075188 E-06 
0 . 291 1221362E-06 
0 . 126 929 3021 E-07 

0. 1 28 3218668 E-ll 
0 . 6  5227 29 441 E-06 
O.OOOOOOOOOOE+OO 
0.121 06 98 470 E-05 
-0.6471117447E-06 
-0.6867469453E-08 


-0 . 257 5687868 E-05 
0 . 2760571526E-08 
-0.27 5227 9831 E-05  RUDDER 

O.OOOOOOOOOOE+OO 
0 .276 49 35447 E-05 
-0 . 1 165974829 E-C5 

-0 . 442251 87 9 3 E- 01 
0.4168 96 198 3 E-04 
-0. 4510255945 E- 01  PORT 

O.OOOOOOOOOOE+OO  BOH  PLANE 
0.453006 46 31 E- 01 
-0 . 17 47 02249  IE- 01 

-0 . 4426664311E-01 
0.4182848952E-04 
-0.4518 982016 E-01  STBD 

O.OOOOOOOOOOE+OO  BOH  PLANE 
0 . 4538842427E-01 
-0 . 1753074334E-01 

0.6050596839E-01 
-0 . 4217829036 E- 03 
0 . 2348 1 4988 5E+00  STERN 

O.OOOOOOOOOOE+OO  PLANE 

-0 . 236 31 29821 E+00 
0 . 1855811163E+00 

0 . 1382443897E-07 
-0 . 293450826 9 E-10 
0 .21 8406 7 56 3 E-07  RPM 

O.OOOOOOOOOOE+OO 
-0 . 2195747233E-07 
0.1 26 921 48 02 E-07 

-0 . 1916238257 E-06 
0.2538 939491 E-10 
-0 . 1217 387 526 E-06  BUOYANCY 
O.OOOOOOOOOOE+OO 
0.12207 47899 E-06 
-0.6880516105E-08 


model  generation  it  should  be  noted  that  bow  plane  forces 
and  moments  are  more  subtle  and  should  be  used  for  fine 
depth  control  rather  than  for  major  depth  excursions. 
Considering  the  speed  mismatch,  the  overall  control  was 
considered  acceptable. 

E.  EFFECT  OF  TIGHTER  FITCH  CONTROL  WEIGHTING 

Due  to  the  unique  dynamics  of  the  vehicle  it  was  decided 
to  leave  th  •'•ntrol  weights  the  same  in  the  E  matrix,  as  it 
was  in  the  first  run,  with  the  understanding  that  the  model 
maneuver  algorithm  perhaps  wasn't  as  well  suited  for  this 
vehicle  as  it  could  have  been. 

With  the  E  matrix  fixed,  with  equal  weights  on  the  bow 
and  stern  planes,  it  was  decided  to  adjust  the  weights  in 
the  Q  matrix  to  try  to  gain  better  control  over  the  pitch, 
and  to  increase  the  pitch  error  gains.  The  weights  that 
were  adjusted  were  those  that  related  pitch  errors,  elements 
Q(ll,ll) ,  C(ll, 16) ,  Q ( 16 , 11) ,  Q ( 16 , 16) . 

When  these  elements  were  increased  by  a  factor  of  1000 
the  pitch  error  gains  increased  and  a  tighter  control  over 
pitch  was  achieved  as  shown  in  Figures  6.13  to  6.17. 

Comparing  Figures  6 . 7  and  6 . 17  shows  the  tighter  control 
gained  over  the  pitch.  With  the  tighter  control  gained  in 
pitch  a  slight  degradation  in  depth  control  was  observed. 
Figures  6.14  and  6.15  show  a  small  overshoot  in  ordered 
depth  for  the  vehicle  indicating  the  loosening  of  the  depth 


ntrol  modes. 
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Figure  6.13  Tighter  Pitch  Control  Weighting  Stern  Plane  Angle 
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Figure  6.14  Tighter  Pitch  Control  Weighting  Dive  Profile 
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Figure  6.16  Tighter  Pitch  Control  Weighting  Bow  Plane  Angle 


o 


76 


Figure  6.17  Tighter  Pitch  Control  Weighting  Pitch  Angle 


F.  FURTHER  PITCH  CONTROL  WEIGHTING 

The  £  matrix  pitch  elements  were  further  increased  by  a 
factor  of  10  to  observe  the  correlation  between  depth  and 
pitch  control.  Again  Figures  6.18  to  6.22  show  a  sluggish 
response  in  depth  control  while  gc.ining  c  much  tighter 
control  over  pitch.  However,  in  this  case  the  command  for 
the  bow  planes  have  exceeded  their  physical  limits  and  are 
commanded  to  an  unreasonable  amount  as  shown  in  Figure  6.21. 

As  the  linear  controller  can  command  a  control  action 
greater  than  the  physical  limits  of  the  vehicle,  when  poor 
weights  are  selected,  logic  was  added  to  the  DSL  code  to 
limit  the  plane  action  to  plus  or  minus  .6  radians  on  the 
bow  and  stern  planes. 

Although  the  increased  weights  in  the  £  matrix  gave  a 
better  pitch  response,  its  effects  on  tracking  control  were 
undesirable.  For  this  reason,  and  for  all  subsequent 
numerical  experiments,  it  was  decided  to  use  the  gains 
originally  calculated  in  the  baseline  run  and  the  original  2 
matrix  weights. 

G.  EFFECT  OF  SPEED  MISMATCH  MODEL/VEHICLE 

The  major  issue  of  control  robustness  relates  to  the 
seriousness  of  speed  mismatch  between  model  and  AUV.  So  the 
next  task  was  to  test  this  controller  over  a  range  of 
vehicle  speeds,  3,  12  and  30  feet/sec. 

Using  the  controller  and  model  based  on  a  speed  of  6 
ft/sec,  the  simulation  was  run  using  a  vehicle  speed  of  12 


Figure  6.19  Further  Pitch  Control  Weighting  Dive  Profile 
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ft/sec.  Figures  €.23  to  6.27  show  very  good  tracking 
ability  even  though  the  vehicle  was  going  twice  the  speed. 
Figure  6.24  shows  that  the  vehicle  went  twice  as  far  as  the 
model  to  reach  the  same  depth,  due  primarily  to  the  vehicle 
speed  being  double  that  of  the  model.  Figure  6.27  shows  the 
compensation  in  pitch  angle  to  achieve  desired  depth.  If 
the  controller  was  tighter  in  pitch  it  would  have  followed 
closer  in  this  figure  but  in  Figure  6.25  the  accurate 
trajectory  tracking  would  be  lost.  Again  this  goes  back  to 
the  type  of  control  needed  and  adjusting  of  the  weights  in 
the  Q  and  £  matrix  to  generate  satisfactory  control  gains. 

The  next  test  of  the  controller  was  an  attempt  to  run 
the  vehicle  at  a  speed  of  3  ft/sec  which  is  very  slow  and 
yet  try  to  use  a  model  speed  of  6  ft/sec.  The  primary 
motivation  was  to  see  if  one  set  of  gains  and  one  model 
could  be  used  for  all  maneuvers,  rather  than  recalculating 
gains  every  time  the  vehicle  changed  speeds;  a  test  of 
robustness  in  the  controller.  When  the  vehicle  was  operated 
at  3  ft/sec  the  vehicle  started  out  lagging  the  model  and 
then  control  errors  grew  while  the  controller  commanded  more 
and  more  action.  But,  since  the  vehicle  was  much  slower 
than  the  model  ordered,  depth  and  path  following  could  not 
be  achieved. 

To  alleviate  this  problem,  gains  were  recalculated  and 
the  model  was  run  at  3  ft/sec  (Figures  6.28  to  6.32)  when 
excellent  tracking  was  restored.  However,  at  the  slower 
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Figure  6.24  Speed  Mismatch  12  Ft/Sec  Dive  Profile 


Figure  6.26  Speed  Mismatch  12  Ft/Sec  Bow  Plane  Angle 


Figure  6.27  Speed  Mismatch  12  Ft/Sec  Pitch  Angle 


Figure  6.29  Vehicle  and  Model  3  Ft/Sec  Dive  Profile 


Figure  6.32  Vehicle  and  Model  3  Ft/Sec  Pitch  Angle 


spaed  (only  1.75  knots)  ths  dlvs  maneuver  algorithm  used  was 
not  sufficient  to  maintain  pitch  during  a  diving  trajectory. 
The  buoyant  moment  overcame  the  hydrodynamic  moment  from 
control  surfaces  and  ordered  depth  was  not  achieved.  This 
behavior,  however,  is  not  characteristic  of  the  controller 
but  rather  ■the  maneuver  logic,  and  as  far  as  the  controller 
is  concerned  it  was  able  to  follow  the  model  rather  nicely. 

Since  the  methodology  here  was  to  design  a  controller 
that  was  robust  enough  to  handle  a  wide  variety  of  reflexive 
type  maneuvers  over  a  range  of  speeds  it  is  most  likely  that 
the  vehicle  will  be  travel  ing  at  much  greater  speeds  when 
these  maneuvers  are  executed.  For  this  reason,  another 
simulation  run  was  made.  Again  the  control  weights  and 
gains  used  were  as  per  the  baseline  case  of  6  ft/sec.  The 
model  was  also  at  6  ft/sec  and  this  time  the  vehicle  was  at 
30  ft/sec.  Figures  6.33  to  6.37  show  once  again  excellent 
tracking  control,  and  like  the  12  ft/sec  case  tight  pitch 
control  was  eased  in  favor  of  accurate  depth  and  trajectory 
control,  which  is  desirable  not  to  have  the  vehicle 
violently  pitching  during  a  maneuver  which  may  result  in 
vehicle  equipment  damage. 

H.  EIGENVALUES — LINEAR  MODEL 

The  following  presents  a  table  of  the  eigenvalues  of  the 
baseline  model  at  6  ft/sec  forward  speed  together  with  the 
clos  d  loop  eigenvalues  found  using  the  baseline  weiahts. 


Figure  6.33  Speed  Mismatch  30  Ft/Sec  Stern  Plane  Angle 


Figure  6.35  Speed  Mismatch  30  Ft/Sec  Depth  Vs  Time 
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Figure  6.37  Speed  Mismatch  30  Ft/Sec  Pitch  Angle 
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Eigenvalues 

1 

2 

3 

4 

5 

6 

7 

8 
9 

1C 

11 

12 

13 

14 

15 

16 

17 

18 


TABLE  4 

OPEN  LOOP  EIGENVALUES 

Bsal  Part 

“0 . 1600E-02 
“0 . 1500E-02 
-0.1700E-02 
-0.1800E-02 
-0.2100E-02  . 

-0 . 1000E-03 
-0.1663E+01 
-0.6579E+00 
-0.9553E+00 
-0.1122E+00 
-0.1122E+00 
-0 . 3909E+00 
-0 . 1603E-01 
-0.9553E+00 
-0 . 3908E+00 
-0. 1423E-01 
-0. 3000E-03 
-0.4000E-03 
-0.5000E-03 


Imaginary  Part 
0.0000E+00 
0 . OOOOE+OO 
C.QQ00E+00 
0. 0000E+00 
0. 0000E+00 
0. OOOOE+OO 
0. OOOOE+OO 
0. OOOOE+OO 
0. OOOOE+OO 
0 . 7003E-02 
0.7003E-02 
0. OOOOE+OO 
0. 0000E+00 

0. 0000E+00  j 

0. 0000E+00 
0. 0000E+00 

t 

0. OOOOE+OO  | 

0. 0000E+00  j 

0. 0000E+00  ' 
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TABLE  5 


CLOSED  LOOP  EIGENVALUES 


-0.1600E-02 


0.0000E+00 


-0.1500E-02 


0.0000E+00 


-0.1700E-02 


0.0000E+00 


-0 . 2100E-02 


0 . 0000E+00 


-0.1663E+01 


0 . 0000E+00 


-0 . 9888E+00 


O.OOOOE+OO 


-0.3456E+00 


0 . 4402E+00 


-0.3454E+00 


“0 . 4402E+00 


-0. 657SE+00 


O.OOOOE+OO 


-0.4868E+00 


O.OOOOE+OO 


-0 . 9553E+00 


O.OOOOE+OO 


-0.1122E+00 


0 . 7003E-02 


-0.1122E+00 


-0.7003E-02 


-0 . 3908E+00 


O.OOOOE+OO 


-0.1423E-01 


O.OOOOE+OO 


-0.1000E-03 


O.OOOOE+OO 


-0.3000E-03 


O.OOOOE+OO 


-0 . 4000E-03 


O.OOOOE+OO 


-0 . 5000E-03 
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A.  SUMMARY 


This  thesis  presents  a  study  of  Model  Reference  Controls 
for  an  Autonomous  Underwater  Vehicle.  The  approach  to  the 
design  and  testing  of  a  model  following  autopilot  included: 

1.  Selection  of  a  suitable  submersible  wa?  selected,  one 
that  displayed  many  attributes  for  potential  AUV 
missions.  One  in  which  all  the  hydrodynamic 
characteristics  were  well  studied  and  data  were 
obtainable. 

2.  The  tailoring  of  the  existing  equations  of  motion  to 
gain  control  over  all  six  degrees  of  freedom. 

3.  The  development  of  a  linearized  model  and  programming 
the  linearized  and  non-linear  models  for  simulation 
using  Dynamic  Simulation  Language  (DSL) . 

4.  The  development  of  a  19  state  controller  for  dive 
plane  maneuvers.  Maneuvers  that  could  be  termed 
reflexive. 

5.  The  development  of  logic  for  a  command  generation 
system  for  a  dive  maneuver. 

6.  Observation  of  the  effects  on  the  control  gains  by 
varying  the  weights  in  the  minimizing  function  J. 

7.  The  testing  of  the  command  generation  logic  and  the 
controller  over  a  wide  range  of  speeds  using  only  one 
set  of  calculated  gains  based  on  one  speed  of  6 
ft/sec. 


B.  CONCLUSIONS 

In  this  study,  a  methodology  was  developed  to  the  design 
of  a  model  following  autopilot  that  could  be  used  in  an 
Autonomous  Underwater  Vehicle.  A  19  state  controller  was 
designed  for  automatic  control  of  maneuvers  in  the  dive 
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plane.  This  controller  displayed  excellent  trajectory 
following  characteristics  and  exhibited  a  high  degree  of 
robustness  over  a  five  to  one  speed  range. 

The  model  following  autopilot  was  designed  to  follow 
trajectories  generated  from  a  preprogrammed  maneuver 
algorithm.  This  maneuver  logic  proved  to  be  workable  and 
could  easily  be  developed  for  a  wide  variety  of  maneuvers  to 
be  stored  on-board  in  a  computer. 

In  this  study  maneuver  logic  was  created  for  one  such 
maneuver,  a  dive  maneuver,  and  was  followed  by  the  designed 
autopilot.  The  algorithm  used  for  the  dive  maneuver  was 
crude  but  sufficiently  proved  that  the  design  methods  are 
sound . 

Some  difficulties  in  perfect  trajectory  following  occur 
because  of  speed  mismatch  between  model  and  vehicle,  and  an 
improvement  in  modelling  speed  loss  during  maneuvers  would 
be  worthwhile. 

C.  RECOMMENDATIONS 

Because  the  concept  of  Autonomous  Underwater  Vehicles  is 
fresh  and  significant  progress  has  been  made  in  the 
computational  abilities  of  modern  computers,  a  wide 
diversification  of  technological  avenues  need  to  be 
explored.  Specific  to  this  study  the  following 

recommendations  are  presented. 

1.  An  implementation  of  the  full  30  state  dynamically 
coupled  controller  in  an  AUV  should  be  the  ultimate 
goal  of  this  project.  In  particular,  the  influence 
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for  forward  speed  changes  should  be  reflected  in  the 
maneuver  command  generation  model  for  greater  control 
accuracy . 


2.  Parallel  efforts  should  be  carried  forward  with  the 
development  of  many  maneuver  algorithms  that  could  be 
stored  in  the  AUV's  "bag"  of  maneuvers. 

3.  Although  this  controller  was  designed  specifically  for 
the  control  of  an  AUV  with  t n  unusual  geometry,  it  can 
and  should  be  tested  on  underwater  vehicles  with  other 
geometries . 


104 


APPENDIX  A 

SIX-DEGREE-OF-FREEDOM  EQUATIONS  OF  MOTION 
AND  EULER  ANGLE  RATES 


SURGE  EQUATION  OF  MOTION 

•  •  • 
m[u  -  vr  +  wq  -  xG(q2  +  rr)  +  yG(Pq  -  r)  +  zG(pr  +  q) ] 

"  ^  p2  +  Xqq  q2  +  Xj-.r  r2  +  ^pr  Pl  3 

+  §  *3CXA  w  +  xwg  wq  +  Xyp  vp  +  Xvr  vr 

+  u<J(xq6s  +  xq6b/2  °bp  +  xq<5b/2  <Sps 

+  X^5r  ur6r]  (A-l) 

+  £■  a2  v2  +  xjw  w2  +  Xv*6r  uv 6r 

•  t 

+  uw(XW£S  6S  +  Xygp/2  5bs 
*  6 

+  xw<Sb/2  bp3 

+  u2(x6s<5s  +  x6b<Sb/2  ^bp  +  x<5b<5b/2  '"’bs 

+  x  <$r  <5r  6r )  3 

-  (W-B)  sin  6  +  |  i3  Xq6sn  uq<$s  e(n) 

+  -j- 2,2  [Xw5sn  uw6 s  +  u2  <5g]  e  (n) 

+  —  2.2  n2  y 

2  ^  u  Aprop 
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A 

■a 


« 

V 

V 


■j 

i 


HEAVE  EQUATION  OF  MOTION 

•  •  * 
m[w  +  uq  +  vp  +  XQ(pr  -  q)  +  yG(qr  +  p) 


1 

H 


-  *g(p2  +  q2)J 

i  • 

§  **[Zq  q  +  2pp  p2  +  Zpr  pr  +  Zrr  r2] 

+  |  i3[Z^  w  +  z£  uq  +  Z^p  vp  +  Zyr  vr] 

+  £  l2  [ZW  UW  +  Zyy  v2 

+  u2(Z{s  Sa  +  Zfik/2  fipg  +  zfib/2  ^bp] 
*hoae 

“  §  /  [Coy  h(x)  (v+xr)2 
*fcail 

+  CDz  b(y)(w-xq)2)  <ix 

+  (W-B)  cos  6  cos  <(> 

+  £  P  *qn  uq 

+  2  J^tZvn  uw  +  z<5sn  «2  «s3  e  (n) 


(A-3) 


ROLL  EQUATION  OF  MOTION 


IxP+  (*z  -  ly)  qr  +  Ixy(pr  -  q)  -  Iyz(q2  -  r2) 

•  •  • 

*  lxz(pq+r)  ♦  tttyc(w  -  «q  +  VP)  -  zG(v  +  ur  -  wp) J 

«  • 

?  A5[Kp  p  +  KJ.  r  +  Kpq  pq  +  Kqy  qr) 

p  * 

+  ?-*4[K^  V  +  Kp  up  +  Kr  ur  +  Kyq  vq  (A-4) 

+  Kwp  wp  +  Kyx  wr) 

p 

+  IA3[lci  uv  +  K^f  vw  +  u2(K^b/2  6bp  +  Kfib/2  ‘W  3 

+  (ycw  “  Ybb)  cos  6  cob  ♦  “  (*GW  “  *Bb)  cos  9  ain't* 

+  f  *4  Kpn  «P  e  (n> 

+  f  i3  U2  Kprop 
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PITCH  EQUATION  OF  MOTION 

xy  q  +  (JX  "  Iz>  Pr  -  Ixy(<Jr  +  P)  +  Iy2(pq  "  r) 

+  Ixz(p2-r2)  “  ®[Xg(v  -  uq  +  vp)  -  zG(u  -  vr  +  wq] 

_  V  •  |  I  I 

■  2  <J  +  Mpp  P2  +  Mpr  pr  +  Mrr  r2] 

+  |  *4[M$  W  +  Mq  uq  +  M^p  vp  +  Myr  vr] 

+  £  A3[M^  uw  +  M^y  v2 

+  u2(M<5s  &a  +  Mfib/2  6bp  +  M6b/2  6bs)  3 

xno8e 

+  f  /  CcDy  Mx)  (v+xr)2 

xtail 

+  CD2  b(x)  (v-xq)2]  x  dx 

“  (xqW  -  xBB)  cos  8  cos  <}>  -  (zGW  -  zbB)  sin  6 
+  f  i4  «i,  uq  e(n) 

+  f  *3tM*ln  uv  +  Mfi'sn  U2  6S]  e(n) 

I 


*aj«  -%m  ■  -vb  n**  -v  •  w  w,;irfr  w if* »'  if'.1  ^\j  irv  w  ■ 


YAW  EQUATION  OF  MOTION 

I*  r  +  (Iy  -  Ix)pq  -  Ixy(P2-q2)  -  Iyz(pr+q) 

•  • 

+  ^(^“P)  +  ®CXq(v  +  ur  -  vp)  -  yG(u  -  vr  +  wq)  ] 
-  f  *5[N$  p  +  Nj  r  +  Npq  pq  +  Ngr  qr] 

I 

+  f  *4[Ny  v  +  Np  up  +  Ny  ur  +  Nyq  vq 

I  I 

+  Nyp  wp  +  Nwr  wr] 

+  £  *-3[Nv  uv  +  Nvv  vw  +  N5r  u2  <sr] 

“  f  /  n°Se  [Coy  h(x)  (v+xr)2 
xtail 

+  CD2  b(x)  (w-xq)2]  0^(1)  xdx 
+  (XqW  -  xbB)  cos  6  sin<j>  +  (yGW  -  yBB)  sin  6 

+  ^  #3  u2  jj 
2  ^  u  wprop 


% 


SJCTTSCK  HTVW  n  V  71  »-n  m.n  BU-*  ».*l  Bnt.nmn^  «■  k  n  u  «-»  w  « 


Inertial  Position  Rates 

x0  *  uc0  +  vi  cos  \p  cos  8 

+  v[oos  ip  sin  0  sin  $  -  sin  ip  cos  tj>] 

+  w[cos  i|j  sin  6  cos  <p  +  sin  i p  sin  <{>] 

* 

y0  *  vc0  +  u  sin  cos  8 

+  v[sin  ^  sin  8  sin  <p  +  cos  \p  cos  4>] 

+  w[sin  \p  sin  8  cos  ^  -  cos  \p  sin  <j>] 

• 

Zq  ■  T*C0  ■  u  sin  8 

+  v  cos  0  sin  ip 
+  w  cos  9  cos  <{> 

Crossflow  Velocity 
ucf(x)  =  [(v  +  xr)2  +  (w  ~  xq)2]1/2 


;  ^x^yrr.vn  V7  u>  v>  vxvv  wwmmwilWITO'WWr.TftW 


APPENDIX  B 

DSL  LISTING  FOR  AUV  SIMULATION 


TITLE 

D 

D 

D 

D 

D 

FIXED 


AUTONOMOUS  UNDERWATER  VEHICLE  (AUV)  SIMULATION 
C0MM0N/BL0CK1/  MMINV(6,6),  MM(6,6),  AA(12,12),  88(6,6) 

- /. - *  *  ,  UM0D(6)  ,GKK(6^21)  ' 

0CF(4) 

,(4)  ,HH(4) 

DOTl 


F#(6  , 
GK4 ( 4 ) , 


COMMON/ BL0CK2/  B(6,6),A( 
C0MM0N/BL0CK3/  F 

COMMON/BLOCK4/  G  _ 

C0MM0N/BL0CK5/  XD0T(12) ,XDOTX 
N , IA , IDGT , IER , LAST , J , K , M , JJ  “ 

INTEGER 

ARRAY  WKAREA(54) ,  X(12) 

A 

CONST 
* 

* 

A 


sr 


XD0TU(6) 


LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 


CONST 

XPP  = 

■88: 
t  Any 

,  XRR  = 

,  XPR  = 

XUDOT= 

,XVP  = 

,  XVR  = 

XQDS- 
xww  = 

,XQDB= 

,XVDR= 

, XRDR= 

,XVV  - 

,xwds= 

,XWDB= 

A 

XDSDS= 

XWDSN= 

,XDBDB= 

,XDSDSN= 

,XDRDR= 

,XQDSN= 

* 

A 

LATERAL 

HYDRODYNAMIC  COEFFICIENTS 

CONST 

YFDOT= 

,YRDOT= 

,YPQ  = 

Ml 

YVDOT= 

,YP  = 

,YR  = 

YWP  * 
YDR  * 

,YWR  = 

,  CDY  * 

,YV  = 

* 

* 

* 


NORMAL  HYDRODYNAMIC  COEFFICIENTS 


,  •  •  « 
f  •  •  • 
/  •  •  • 


CONST 

ZQDOT= 

,ZPP  = 

,ZPR  = 

,ZRR 

S 

/  •  •  • 

ZWDOT= 

,ZO  * 

,ZVV  = 

,ZVP  a 

,ZVR 

8 

t  •  •  • 

ZW  = 

,ZDS  = 

,  ZD3 

= 

9  •  •  • 

A 

ZQN  * 

,  ZWN  = 

,ZDSN= 

,  CDZ 

8 

* 

it 

ROLL  HYDRODYNAMIC  COEFFITIENTS 

CONST 

KPDOT= 

,  KRDOT= 

,  KPQ  = 

,KOR 

S 

t  •  •  • 

KVDOT- 

,  KP  = 

,KR  = 

9  •  •  • 

KWP  = 

,  KWR  = 

,KV  = 

= 

/  •  •  • 

* 

KPN  = 

,  KDB  = 

* 

A 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

CONST 

MQDOT= 

,  MPP  = 

,MFR  = 

,MRR 

= 

/  ■  •  • 

MWDOT= 

:8Sv*. 

,  MVP  = 

,MVR 

8 

f  *  •  • 

mw  = 

,MDS  = 

,MDB 

= 

9  •  •  • 

it 

MQN  = 

,  MWN  = 

,MDSN  = 

* 

it 

YAW  HYDRODYNAMIC  COEFFICIENTS 

CONST 

NPD0T= 

,  NRD0T= 

,NPQ  = 

,NQR 

= 

9  •  •  • 

NVDOT= 

,  NP  = 

,NR  = 

,NVO 

,NVW 

8 

9  •  •  • 

NWP  = 

,  NWR  = 

,NV  = 

= 

9  •  •  • 

A 

NDR  = 

A 

it 

MASS  CHARACTERISTICS  OF  THE  FLOODED  auv 

CONST 

WEIGHT  = 

,  BOY  = 

,  VOL  = 

,XG  = 

- 

9  •  •  • 

YG  = 

,  ZG  = 

,XB  - 

,ZB  = 

= 

f  •  •  • 
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I*  -1C  *  f-ICNHC  *  * 


L  =  ,  RHO  =  ;g  = 

AO  =  , KPROP  =  ,NPROP  = 

INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 


NU 
X1TEST 


* 


* 


INITIAL 


INITIALIZE  ALL  MATRICES  AND  ARRAYS  TO  ZERO 


N  =  6 

DO  2  J  =  1 ,N 
JJ=  J+N 
DO  1  K  =  1,N 
KK=  K+N 
KKK=  KK  +  N 
MMINV(J.K)  =  0.0 

x(j)  =  6.6 
X(JJ)  =  0.0 
XDOT(J)  =0.0 
XDOT(JJ)  =0.0 
XDOTX(J)  =0.0 
XDOTX(JJ)  =0.0 
XDOTU(J)  =0.0 
UMOD(J)  =0.0 
MM{J,K)  =  0.0 
BB(J,K)  =  0.0 
B(J,K)  =  0.0 
AA( J ,K)=  0.0 
AA( JJ ,KK)=  0.0 
AA( J ,KK)=  0.0 
AA( JJ,K)=  0.0 
'  A ( J , KK ) =  0.0 
A(JJ,K)  =  0.0 
A ( J , K )  =  0.0 
A(JJ,KK)=  0.0 
GKK ( J , K ) =  0.0 
GKK( J ,KK)=0 . 0 
GKK (J , KKK ) =0 . 0 
CONTINUE 

CONTINUE 


INPUT  THE  LINEARIZATION  POINT  PARAMETERS 

UO  =6.0 
VO  =  0.0 
WO  =  0.0 
PO  =  0.0 

go  =  o.o 

RO  =  0.0 
PHIO  =  0.0 
THETAO  =0.0 
PSIO  =  0.0 
SUM  =  0.0 
JFLAG  =  0 
I FLAG  =  0 
KFLAG  =  0 
ZORD  =  100.0 

INPUT  THE  MODEL  STATES  INITIAL  CONDITIONS 

UM  =  6.0 
VM  =  0.0 
WM  =  0.0 
PM  =  0.0 
OM  =  0.0 
RM  =  0.0 
XPOSM  =0.0 
YPOSM  =0.0 


«««*  ***  ***  *  *  * 


¥^v^vw  wn»<n^v»vivpvntfnvn¥«  \r»  uwvi iiri  ut«  vr»  w 


ZPOSM  *  0.0 
PHIM  =  0.0 
THETAM  *  0.0 
PSIM  =  0.0 


INPUT  THE  VEHICLE  INITIAL  CONDITIONS 

U  =  6.0 
V  *  0.0 
W  =  0.0 
P  =  0.0 
0  =  0.0 
K  =  0.0 

XPOS  =  0.0 
YPOS  =  0.0 
ZP0S  *  0.0 
PSI  ■  0.0 
THETA  =0.0 
PHI  =  0.0 

INITIALIZE  THE  CONTROLS 

DELBOY=  0.0 
DBS-  0.0 
DBP=  0.0 
DS  =  0.0 
DR  =  0.0 
RPM  =  250.00 
LATYAW  =0.0 
NORPIT  =0.0 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

G4(l)  =  0.069431844 
G4{2)  =  0.330009478 
G4(3)  =  0.669990521 
G4(4)  =  0.930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADUTURE  TERMS 

GK4 ( 1 )  =  0.1739274225687 
GK4{2)  =  0.3260725774312 
GK4(3)  =  0.3260725774312 
GK4(4)  =  0.1739274225687 

DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 

BR(1)  =  75.7/12 
BR(2)  =  75.7/12 
BR(3 j  =  75.7/12 
BR(4)  =  55.08/12 

HH(1)  =  16.38/12 
HH(2 )  =  31.85/12 
HH(3 )  =  31.85/12 
HH(4)  =  23.76/12 

MASS  =  WEIGHT/G 

DIVAMP  =  DEGSTN^O .0174532925 
RUDAMP  =  DEGRUD^Q .0174532925 


THE  LINEAR  PROPULSION  MODEL 


ETA  =  0. 012*500. 0/U0 
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* 


* 

* 

* 


A 


A 


A 


A 


A 

A 


20 

A 


A 

A 


A 

A 


ETA  =  1.0 


RE  =  U0*L/NU 

CDO  =  .00385  +  (1.296E-17)*(RE  -  1.2E7)**2 

Si :  o:oo!uili/(Ao)ABS("A>/<AO) 

"lo?  =1CDoi(®2s<ETlj1:°i{J)8RT<CTltl-0>' 


1.0) 


MASS  =  WEIGHT/G 

DIVAMP  =  DEGSTN*0. 0174532925 

RUDAMP  =  DEGRUD*0. 0174532925 


CALCULATE  THE  MASS  MATRIX 


MAll^-^RHO/2)  *  (L**3 )  *XUD°T) 
-MASS*YG 


MM(2 

MM(2 


MM(2 


=  MASS  - ((RHO/2 ) * (L**3) *YVD0T ) 

=  -MASS*ZG  -( (RHO/2 )*(L**4)*YPD0T) 
=  MASS*XG  -  (  (RHO/2 )*(L**4)*YRD0T) 


MM(3 

MM(3 

MM(3 


=  MASS  -  ( (RH0/2)*(L**3)*ZWD0T) 

=  MASS*YG 

=  -MASS*XG  -(<RHO/2)*(L**4)*ZQDOT) 


MMI 
MM  I 
MMI 
MM  I 
MMI 


-MASS*ZG  -  ( (RHO/2 )*(L**4)*KVD0T) 
MASS*YG 

IX  -  ((RHO/2)*(L**5)*KPDOT) 

-IXZ  - ( (RHO/2)*(L**5)*KRDOT) 


MM 

MM 

MM 

MM 

MM 


MASS*ZG 

-MASS*XG  -( (RHO/2 )*(L**4)*MWD0T) 
IY  -( (RHO/2 )*(L**5)*MQDOT) 


MMI 

MMI 

MMI 

MMI 

MMI 


a  -MASS*YG 

=  MASS*XG  -( (RHO/2 )*(L**4)*NVD0T) 
=  -IXZ  -  ((RHO/2)*(L^*5)*NPDOT) 

*  IZ  -  ((RHO/2)*(L**5)*NRDOT) 


LAST  *  N*N+3*N 
DO  20  M  =  1 , LAST 
WKAREA(M)  =  0.0 
CONTINUE 

IER  =  0 
IA  =  6 
IDGT  =  4 

WRITE (  8,400) ( (MM(I, J) ,  J  =  1,6), I  =  1,6) 
CALL  LINV2F (MM , N , IA , MMINV , IDGT , WKAREA , IER) 

Foffi(!E12°^<H,1INV<I'J)'  "  '  1'6>'1  =  l-S)' 


CALCULATE  THE  A  MATRIX  FOR  THE  LINEAR  MODEL 


A(l,l)  = 


RHO/2*L**3*^XQDS*DS*QO+XQDB/2*DBP’,fQO+XRDR^RO’,'DR)  + .  .  . 
R20/??L  2*(WDR*VO*DR+XWDS*DS*WO+XWDB/2*DBP*WO  +  .. 
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aasstBiiim^ 


XQDB/2*DBS*QO+RHO/2*L**2*XWDB/2*DBS*WO+RHO*L**2*UQ*. . . 
XDBDB/2*DBS**2 

A(l,2)  =  MASS*RO+RHO/2*L**3*(XVP*PO+  XVR*RO)  +  RHO/2*L**2*  ... 
(2*XW*V0  +  XVDR*UO*DR) 

A(l,3)  *  -MA3S*00  +  RHO72*L**3*(XWQ*Q0)+RHO/2*L**2*(2*XWW*W0+ . . . 

XWDS*D**U0+(XTOB/2*DBP+XWBB/ 2*DBS)*U0  +XWDSN*UO*DS*EPS) 
A(l,4)  *  “MASS*YG’l'QO“MASS*ZG*P.O+  RHO/2*L**4*(2*XPP*PO+XPR*RO) . . . 


A(l,6) 

Ml, 11) 


=  MASS*VO+2*MASS*XG*RO“MASS*ZG,I'PO+RHO/2*L**4*(2*XRR*RO . . . 

+  XPR*PO)  +  RHO/2*L**3*7XVR*VO  +  XRI)R*UO*DR) 

=  -(WEIGHT  -  BOY ) *COS ( THEtAO ) 


A(2,l)  =  -MASS*RO+RHO/2*L**3*(YP*PQ+YR*RO)+RHO/2*L**2*(YV*VO+. . . 
2*YDR*UQ*DR) 

A(2,2)  =  RHO/2*L**3,l'xVQ*QO+RHO/2*L**2*(YV*UO+YVW*WO) 

A(2,3 )  =  MASS*P0+  RHO/Z*L**3*7yWP*PO+YwR*RO)+RHO/2*L**2*YVW*VO 
A(2,4)  =  MASS*WO-MASS*XG*QO+2*MASS*YG*PO+RHO/2*L**4*YPQ*QO+. . . 
RHO/2*L**3*(YP*UO+  YWP*WO) 

A(2, 5)  =  -MASS*XG*PO-MASS*ZG*RO+RHO/2*L**4*(YPQ*PO+YQR*RO)  +... 
RHO/2*L**3*YVQ*VO 

A(2 ,6)  =  “MASS*UO+2*MASS*YG*RO“MASS*ZG*QO+RHO/2*L**4*YQR*QO  +... 

RHO/ 2*L**3* ( YR*UO  +  YWR*W0) 

A(2,10)=  (WEIGHT  -  BOY) *COS (THETAO )*COS( PHI 0) 

A(2,ll)=  -(WEIGHT  -  BOY)*SIN(THETAO)*SIN(PHl6) 


A(3,l)  =  MASS*QO+RHO/2*L**3*ZQ*QO+RHO/2*L**2*(ZW*WO+2*UO*ZDS*DS. 

+2*U0*ZDB/2*DBP+ ( ZWN*W0+2*ZDSN*U0*DS ) *EPS ) +RHO/2*L**3* . 
ZQN*00*EPS+  RH0/2*L**2*2*U0*ZDB/2*DBS 
A(3,2)  «  -fosS*PO+RHO/2*L**3*(ZVP*PO+ZVR*RO)+RHO/2*L**2*2*ZVV*VO 
A(3,3)  ■  RHO/2*L**2*(ZW*UO  +  ZWN*U0*EPS) 

A(3,4)  =  -MASS*V0-MASS*XG*R0+2*MASS*ZG*P0+  RHO/2*L**4*(2*ZPP* . . . 

PO  +  ZPR*RO)  +  RHO/2*L**3*ZVP*VO 
A(3,5)  =  MASS*UO  -  MASS*YG*RO+2*MASS*ZG*QO+RHO/2*L**3*ZQ*UO  +... 
RHO/2*L**3*ZQN*UO*EPS 

A(3,6)  =“MASS*XG*PO-MASS*YG*QO+RHO/2*L**4*(ZPR*PO+2*ZRR*RO)+. . . 
RHO/2*L**3*ZVR*VO 

A(3,10)=  -(WEIGHT  -  BOY ) *COS ( THETAO )*S IN ( PHI 0) 

A(3,ll)=  -(WEIGHT  -  BOY ) *S IN ( THETAO )*COS( PHI 0) 


A(4, 1)  =  MASS*YG*Q0  +  MASS*ZG*R0  +  RH0/2*L**4*(KP*P0  +  ... 

KR*RO)+RHO/2*L**3*(KV*VO+2*UO*(KDB/2*DBP-KDB/2*DBS) )  + . . 
RHO/2*L**3*UO*KPROP+  RHO/2*L**4*KPN*PO*EPS 
A(4,2)  =  -MASS*YG*P0  +  RH0/2*L**4*KVQ*Q0  +  RHO/2*L**2*(KV*UO . . . 

A(4,3)  =  -MASS*ZG^P0  +  RHO/2*L**4*(KWP*PO  +  KWR*RO)  +  ... 
RHO/2*L**3*KVW*VO 

A(4,4)  =  -IXY*R0  +  IXZ*Q0  -  MASS*YG*V0  -  MASS*ZG*W0  +  ... 

RHO/2*L**5*KPO*Q0  +  RHO/2*L**4*(KP*UO+KWP*WO) 

A(4, 5)  =  -IZ*R0  +  IY*R0  +  2*IYZ*Q0  +  IXZ*P0  +  MASS*YG*U0  +... 

RHO/ 2*L**5* { KPQ*P0  +  KQR*R0)  +  RH0/2*L**4*KVQ*V0 
A(4,6)  *  -IZ*00  +  IY*00  -  2*IYZ*R0  +  MASS*ZG*U0  +  ... 

RHO/2*L**5*KQR*QO  +  RHO/ 2*L**4* (KR*UO+KWR*WO^ 

A(4,10)=  - (YG*WEIGHT-iB*BOY)*COS (THETAO )*S IN (PHI 0) ... 

- (ZG*WEIGHT-ZB*BOY) *COS (THETAO ) *COS  PHIO ) 

A(4,ll)=  -(YG*WEIGHT-YB*BOY)*SIN(THETAO)*COS(PHIO) . .  . 

+ ( ZG*WE I GHT - ZB*BOY  J  *S IN ( THETAO ) *S IN ( PHI 0 ) 


A(5, 1)  =  -MASS*XG*00  +  RH0/2*L**4*MQ*Q0  +  RHO/2*L**3*MW*WO  +... 

RH0/2*L**3*U0*(MDS*DS+MDB/2*DBP)  +  RH0/2*L**4*MQN*Q0* .  . 
EPS  +  RHO/2*L**3*(MWN*WO  +  2*MDSN*U0*DS)*EPS+. . . 

RHO/ 2*L**3*U0*MDB/ 2*DBS 

A(5,2)  =  MASS*XG*PO  +  MASS*ZG*RO  +  RHO/2*L**4*(MVP*PO  +  ... 
MVR*RO)  +  RHO*L**3*MW*VO 

A(5,3)  =  -MASS*ZO*QO  +  RHO/2*L**3*MW*UO  +  RHO/2*L**3*MWN*UO*EPS 
A(5,4)  =  -IX*RQ  +  IZ*RQ  -  IYZ*QO  -  2*IXZ*P0  +  MASS*XG*VO  +  ... 

RHO/2*L**5*(2*MPP*PO  +  MPR*RO)  +  RHO/2*L**4*MVP*VO 
A(5,5)  =  IXY*RO  -IYZ*PO  -  MASS*XG*UO  -MASS*ZG*WO  +  RHO/2*... 


L**4*MQ*U0  +  RHO/2*L**4*MQN*UO*EPS 
A(5,6)  *  -IX*PO  +  IZ*PO  +  IXY*QO  +  2*IXZ*R0  +  MASS*ZG*VO  +... 

RHO/2*L**5*(MPR*PO+2*MRR*RO)+RHO/2*L**4*MVR*VO 
A(5,10)=  ( XG*WEIGHT-XB*BOY ) *COS ( THETAO ) *S IN ( PHI 0 ) 


A(5,li; 

A(6,l) 

A(6,2) 

A(6,3) 

.  A(6,4) 

A(6,5) 

A(6,6)  = 

A(6,10)= 

A(6,ll)= 


( XG*WE IGHT- XB*BOY ) *S IN ( THETAO ) *COS ( PHI 0 )  - 
( ZG*WE IGHT - ZB*BOY )  *COS (THETAO ) 

-MASS*XG*RO  +  RHO/2*L**4*(NP*PO  +NR*RO)  +  RHO/2*... 
L**3*(NV*VO+2*NDR*UO*DR)+RHO*L**3*UO*NPROP 
-MASS*YG*RO  +  RHO/ 2*L**4*NVQ*Q0  +  RHO/2*L**3*(NV*UO+. 
NVW*WO) 

MASS*XG*PO  +  MASS*YG*QO  +  RHO/2*L**4*(NWP*PO+NWR*RO)+ 
RHO/2*L**3*NVW*VO 

-IY*00  +  IX*Q0  +  2*IXY*P0  +IYZ*RO  +  MASS*XG*WO+. . . 
RHO/2*L**5*NPQ*QO  +  RH0/2*L**4*(NP*U0+NWP*W0) 

-  — ’?0  -  2*IXY*Q0  -  IXZ*RO  +  MASS*YG* 


=  -IY*PO  +  IX*P( 


_  _  __  WO+. 

)  +  RHO/ 2*L**4*NVQ*V0 


A< 

A< 

A< 

A< 

A( 


RHO/2*L**5*(NPQ*PO+NQR*RO,  _  _ 

IYZ*P0  -IXZ*QO  -  MASS*XG*(jO  -MASS*YG*V0  +  . 
RHO/2*L**5*NQR*QO  +  RHO/2*L**4*(NR*UO  +NWR*WO) 
(XG*WEIGHT-XB*BOY ) *COS (THETAO ) *COS ( PHIO ) 
-(XG*WEIGHT-XB*BOY)*SIN(THETAO)*SIN(PHIO)  +. . . 
(YG*WEIGHT-YB*BOY)*COS (THETAO) 

COS ( PS I 0 ) *COS ( THETAO ) 

COS (PSIO )*S IN (THETAO )*SIN( PHIO)  -  SIN(PSIO)*COS(PHIO) 
COS ( PS 10 ) *S IN ( THETAO ) *COS (PHIO)  +  SIN(PSI0)*SIN(PHI0) 
VO*COS (PSIO )*SIN( THETAO )*COS (PHIO)  +  VO*SIN(PSIO)*. . . 
SIN(PHIO)  -  WO*COS (PSIO) *SIN (THETAO )*SIN (PHIO)  +  ... 
WO*SIN(PSIO)*COS(PHIO) 

-UO*COS (PSIO) *S IN (THETAO)  +  VO*COS (PSIO )*COS (THETAO)* 
SIN(PHIO)  +  WO*COS (PSIO )*COS (THETAO )*COS (PHIO) 

-UO*SIN ( PSIO) *COS ( THETAO )  -  VO*SIN(PSIOpSIN(THETAO)* 
SIN(PHIO)  -  VO*COS(PSIO)*COS(PHIO)  -  WO*SIN(PSl0)* . . . 
S IN ( THETAO )*S IN (PHIO)  +  WO*COS(PSIO)*SIN(PHIO) 

SIN ( PSIO )*COS (THETAO) 

SIN(PSIO)*SIN(THETAO)*SIN(PHIO)  +  COS(PSIO)*COS(PHIO) 
SIN(PSIQ ) *SIN(THETAO ) *COS (PHIO )  -  COS(PSIO)*SIN(PHIO) 
VO*SIN(PSIO)*SIN(THETAO)*COS(PHIO)  -  VO*COS(PSIO)*. . . 
SIN(PHIO)  -  WO*SIN(PSIO)*SIN(THETAO)*SIN(PHIO)  -  ... 
WO*COS(PSIO)*COS(PHIO) 

-U0*SIN(PSI0)*5IN( THETAO)  +  VO*SIN(PSIO)*COS(THETAO)* 
SIN(PHIO)  +  WO*SIN(PSIO)*COS(THETAO)*COS(PHIO) 

UO*COS (PSIO )*COS (THETAO)  +  VO*COS(PSIO)*SIN(THETAO)*. 
SIN(PHIO)  -  VQ*SIN ( PSIO )*COS( PHIO)  +  WO*COs(PSIO)* .  . . 
S  IN  ( THETAO ) *CQ S ( PHI 0 )  +  WO*SIN(PSIO)*SIN(PHIO) 

-SIN (THETAO) 

COS ( THETAO ) *SIN ( PHI 0 ) 

COS (THETAO ) *COS (PHIO ) 

VO*COS(THETAO)*COS (PHIO) -WO*COS (THETAO )*SIN(PHIO) 
-UO*COS(THETAO)-VO*SIN(THETAO)*SIN(PHIO)  -. . . 
WO*SIN(THETAO)*COS(PHIO) 

10.4)  =  1.0 

10.5)  =  SIN (PHIO ) *TAN( THETAO ) 

10.6)  =  COS ( PHI 0)*TAN( THETAO) 

-  ‘  ~iO*COS (PHIO) *TAN ( THETAO )  -  RO*SIN(PHIO)*TAN(THETAO) 

iO*SIN (PHIO  /COS (THETAO  *1.0/COS (THETAO)  +  ... 

[0*C0S (PHIO ) /COS (THETAO ) *1 . O/COS (THETAO ) 


A<7,11)* 

A(7,12)= 


A(8,l 
A(S,2 
A(8,3l 
A(8,lO)= 


A(8,ll)= 

A(8,12)= 


A(ll , 5)  =  COS (PHIO ) 

A(ll,6)  =  -SIN(PHIO) 

A(ll,lO)=  -QO*SIN(PHIO)  -  RO*COS(PHIO) 

A(12, 5)  =  SIN (PHIO ) / COS (THETAO ) 

A(12,6)  =  COS (PHIO )/COS (THETAO ) 

A(l2,10)=  QO*COS(PHIO)/COS (THETAO) -RO*SIN(PHIO)/COS (THETAO) 
a(12,ii)=  qo*sin(phio)/cos(thetao  *TAN(THETAO)  +  ... 

RO*COS (PHIO ) /COS (THETAO ) *TAN( THETAO ) 


WRITE(10,200)((A(I,J),J*1,12),I*1,12) 
CALCULATE  THE  B  MATRIX 


B(l,l)  »  RH^2*Lj£3*XRDR*UO*RO+RHO/2*L**2MX^R*UO*VO+UO**2*... 

B(1 ,2}  ■  UO*QO*XODb}2  +  U0*W0*XWDB/2  +  U0**2*XDBDB*DBS 
B(1 ,3 )  «  U0*Q0*XQDB/2  +  U0*W0*XWDB/2  +  U0**2*XDBDB*DBP 
B(l,4)  *  UO*QO*XQDS  +  UO*WO*XWDS  +UO**2*2*XDSDS*DS+RHO/2*L**3* . . . 

XQD5N*UO*QO*EPS  +  RHO/2*L**2*(XWDSN*UO*WO  +  2*XDSDSN*... 
U0**2*DS }  *EPS 

B(1 ,5)  «  RHO/2*L**2*0 . 12*CD0*0 . 12*RPM 
B(l,6)  =  SIN(THETAO) 

B(2,l)  ■  RHO/2*L**2*YDR*UO**2 
B(2,6)  a  -COS(THETAO)*SIN(PHIO) 

B(3,2)  «  UO**2*ZDB/2*RHO/2*L**2 
B  3,3/  «  U0**2*ZDB/2*RH0/2*L**2 

B  3,4/  *  UO**2*ZDS*RHO/2*L**2  +  RHO/2*L**2*ZDSN*UO**2*EPS 
8(3,6)  =  -COS (THETAO )*COS(PHIO) 

B(4,2)  a-RHO/2*L**3*UO**2*KDB/2 
B(4,3)  *  RHO/2*L**3*UO**2*KDB/2 

B(4,6)  »  -YB*COS (THETAO ) *COS ( PHI 0 )  +  ZB*COS ( THETAO ) *S IN ( PHI 0) 

B(5,2)  «  RHO/2*L**3*UO**2*MDB/2 
B(5 ,3 )  =  RHO/2*L**3*UO**2*ML3/2 
Bi 5,4  i  »  RHO/2*L**3*(UO**2*HDS+MDSN*UO**2*EPS) 

B(5,6)  a  XB*COS (THETAO )*COS(PHIO)  +  ZB*S IN (THETAO) 

B(6, 1)  ■  RHO/2*L**3*NDR*UO**2 

B(6,6)  *  -XB*COS (THETAO ) *SIN ( PHIO )  -  YB*S IN (THETAO) 

WRITE (  9,300)((B(I,J),Jal,6),I=l,6) 

FORMULATE  THE  A  AND  B  MATRIX  FOR  STATE  SPACE  REPRESENTATION 

MULTIPLY  MMINV  AND  DF/DX 

DO  80  I  =  1,6 
DO  70  J  =  i  fi 
SUM  =  0.6 
DO  60  K  =  1,6 

SUM  =  SUM  +  MMINV(I , K)*A(K, J) 

CONTINUE 
AA(I , J)  a  SUM 
CONTINUE 
CONTINUE 

MULTIPLY  MMINV  AND  DF/DZ 

DO  50  I  =  1,6 
DO  40  J  =  7,12 
SUM  =  0.0 

DO  30  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*A(K,J) 

CONTINUE 
AA(I , J)  =  SUM 
CONTINUE 
CONTINUE 

DO  5  I  =  7,12 
DO  6  J  =  1.12 
AA(I , J)  =  A(I , J) 


6  CONTINUE' 

5  CONTINUE 
* 

* 


WRITE 

FORMA' 


J10,200)^AA(i,j),j»1,12),i=1,12) 


MULTIPLY  MMINV  AND  DF/DU 


DO  110  I  -  1,6 
DO  100  J  =  1,6 
SUM  =  0.0 
DO  90  K  =  1,6 

SUM  =  SUM  +  MMINV(I,K)*B(K, J) 
CONTINUE 
BB(I,J)  =  SUM 
CONTINUE 
CONTINUE 


WRITE(  9,300)((BB(I,J),J=1,6),I=1,6) 
FORMAT (6E12. 4) 


DO  405  I  =  1.6 


U w  TWJ  X  A , W 

READ  (2,401) (GKK(I , J) ,  J=l,21) 

405  WRITE (3 ,401) (GKK(I , J) ,  J=l,21) 

401  FORMAT (3E20. 10) 

A 

DERIVATIVE 

NOSORT 

A 

* 

*****ltnear  model****************************************************** 

A 

*  WRITE (8, 600) 

J00  FORMAT (14HENTERED  DERIV.) 

*  CALCULATE  BB*U  PART  OF  XDOT  =  AA*X  +  BB*U 
A 

DO  10  J  a  1.6 
SUM  =  0.0 
DO  15  K  =  1,6 

SUM  =  SUM  +  BB(J,K)*UMOD(K) 

15  CONTINUE 

XDOTU(J)  =  SUM 
10  CONTINUE 

*  CALCULATE  AA*X 

DO  21  J=  1,12 
SUM  *  0.0 
DO  25  K  =  1,12 

SUM  =  SUM  +  AA(J,K)*X(K) 

25  CONTINUE 

XDOTX(J)  =  SUM 
21  CONTINUE 

*  CALCULATE  XDOT  =  AA*X  +  BB*U 

DO  31  J  =  1,6 

XDOT(J)  =  XDOTX(J)  +  XDOTU(J) 

31  CONTINUE 

DO  35  J  ■  7,12 

XDOT(J)  =  XDOTX(J) 

35  CONTINUE 

A 

UDOTM  =  XDOT { 1 ) 

VDOTM  =  XDOT (2 
WDOTM  =  XDOT ( 3 ) 

PDOTM  =  XDOT (4) 


* 

* 

* 


A 

A 


QDOTM 

RDOTM 

XDOTM 

YDOTM 

2DOTH 

PHMDOT* 

THETMD* 

PSMDOT* 


XDOT 

XDOT 

XDOT 

XDOT 

XDOT 

XDOT 

XDOT 


_  XDOT (12 

WRITE (8  600) 

INTEGRATE' XDOT  TO  GET  THE  STATE  VECTOR  X 

UM  *INTGRL(6.0,  UDOTM) 

VM*  XNTGRL (0.0,  VDOTM 
WM«  INTGRL (0.0,  WDOTM 
PM*  INTGRL (0.0,  PDOTM 
QM*  INTGRL (0.0,  ODOTM 
RM*  INTGRL (0.0.  RDOTM 
XPOSM  -  INTGRL (0.0,  XDOTM) 

YPOSM  *  INTGRL (0.0,  YDOTM) 

ZPOSM  *  INTGRL (0.0,  ZDOTM) 

PHIM  *  INTGRL (0.0,  PHMDOT  i 
THETAM  *  INTGRL (0.0,  THETMD) 

PSIM  *  INTGRL (0.0,  PSMDOT) 


1 

2 

3 

4 

5 

6 

7 

8 
9 
1 

11 

12 


UM 

VM 

WM 

PM 

2M 


XPOSM 

YPOSM 

ZPOSM 

■  PHIM 

■  THETAM 
•  PSIM 


ZDEPTH  *  ZORD  -  X(9) 

THMANG  *  X(ll)*57 .3 
DRM  *  UMOD(l) 

DBSM-  UMOD ( 2 
DBPM=  UMOD (3 
DSM  *  UMOD (4 
DRPM=  UMOD (5 
DBOY*  UMOD ( 6  J 

A 

aaaaaacontrol  law**************************aaaaaaaaaaaaaaaaaaaaaa*aa*aaa 

A 
A 
A 
A 


DBS  =  UMOD (2 
DBP  *  UMOD  (3 
DS  =  UMOD (4 


DBS  *  - (GKK ( 2 , 

GKK(2,5) 

GKK (2,9) 

GKK(2 , 12 
ZPOSM  + 

UMOD (3) 

DBP  =  - (GKK ( 3 , 

GKK (3,5) 

GKK (3,9) 

GKK( 3 , 12 
ZPOSM  + 

UMOD (3) 

DS  =  -(GKK(4. 

GKK (4,5)  ^  _ ,  „ 

GKK(4,9)*2POS  +  GKK(4,10)*PHI  + 


1)*U  +  GKK ( 2 . 2 ) *V  +  GKK ( 2 , 3 ) *W  +  GKK(2,4)*P  +... 

*2  +  GKK (2 , 6  )*R  +  GKK(2,7)*XPOS  +  GKK (2,8) *YPOS  +. 
*ZP0S  +  GKK(2,10  *PHI  +  GKK?2, 11 )*THETA  +  ... 

:)*PSI  +  GKK(2,13)*WM  +  GKK(2,14)*QM  +  GKK(2.15)*.. 

h^(56i9fec:)r(2'^),,UMOD?2>  +  GKK(i'1^*- 

1)*U  +  GKK ( 3 . 2 ) *V  +  GKK(3.3)*W  +  GKK(3.4)*P  +... 

*6  +  GKK(3 ,6;*R  +  GKK(3,7)*XPOS  +  GKK(S , 3)*YPOS  +. 
*2POS  +  GKK(3 , 10)*PHI  +  GKK(3,11)*THETA  + 

)*PSI  +  GKK  3,13)*WM  +  GKK(3A14)*^M  +  GKK 


/  *  aj /  ■  nr*  t 

GKK(3.16)*THETAM  +  GKK(3,17 


+  GKK(3,19)*UMOD(4)) 

i)*U  +  GKK(4.2)*V  +  GKK ( 4 , 3 )  *W  + 

*6  +  GKK ( 4 , 6 ) *R  +  GKK(4 , 7 )*XPOS  + 


: 2 ) 


GKK (3,15)*. . 
+  GKK (3,18)*. 


GKK(4,4)*P  +... 

_■  GKK(4, 8)*YPOS  +, 
GKK (4,11) *THETA  +  ... 


120 


GKK(4,12)*PSI  +  GKK(4,13)*WM  +  GRK(4. 14)*QH  +  GKK(4, 15)* , 
ZPQSM  +  GKK(4.16)*THETAM  +  GKK(4, 17)*UMOD(2)  +  GKK(4,l8)y 
UMOD ( 3 )  +  GkK(4, l9)*UMOD(4) ) 


PUT  IN  STERN  AND  BOH  PLANE  STOPS 

IF(ABS(DBS) .GT.O .6)  THEN 
DBS  ■  0 . 6*ABS (DBS ) /DBS 
ENDIF 

IF(ABS(DBP) .GT.O. 6)  THEN 
DBP  =  0 .6*ABS(DBP)/DBP 
ENDIF 

IF(ABS(DS) .GT.O. 6)  THEN 
DS  ■  0 .o*ABS(DS)/DS 
ENDIF 


******j|qj$-linEAR  MODEL************************************************ 

*  PROPULSION  MODEL 

* 

* 


SIGNU  *-1.0 

IF  (U.LT.O.O)  SIGNU  =  -1.0 
IF  (ABS(U) .LT.X1TEST)  U  =  X1TEST 
SIGNN  =1.0 

IF  (RPM.LT.O .0)  SIGNN  *  -1.0 
ETA  =  0.012*RPM/U 
RE  «  U*L/NU 

CDO  =  .00385  +  (1.296E-17)*(RE  -  1.2E7)**2 

CT  =  0.008*L**2*ETA*ABS(ETA)/(A0) 

CT1  =  0.008*L**2/(AO) 

EPS  =  -1.0+SIGNN/SIGNU*(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP  «  CDO* (ETA*ABS (ETA)  -1.0) 


CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 


LATYAW  =0.0 
NORPIT  =0.0 
DO  500  K  =  1,4 

UCF^Kji  =  SgRT(|V+G4(K)*R*L)**2  +  (W-G4(K)*Q*L)**2) 


TERMO  = 

TERM1  = 
TERM2  = 
LATYAW  = 
NORPIT  = 
END  IF 
CONTINUE 


(RH0/2)*(CDY*HH(K)*(V+G4(K; 
CDZ*BR(K)*(W-G4 ( K j  *Q*L ) **2 | 
TERMO* (V+G4(K)*R*L)/UCF(K) 
TERMO* (W-G4(K)*Q*L)/UCF(K) 
LATYAW  +  TERMl*GK4(K)*L 
NORPIT  +  TERM2*GK4(K)*L 


*R*L) **2  +, . . 


FORCE  EQUATIONS 


surge  FORCE 

FP(1)  =  MASS*V*R  -  MASS*W* 


+  MASS*XG*Q**2  +  MASS*XG*R**2- 


MASS*YG*P*Q  -  MASS*ZG*P*R  +  (RH0/2)*L**4*(XPP*P**2  +... 
XQQ*Q**2  +  XRR*R**2  +  XPR*P*R)  +(RHO/2)*L**3*(XWQ*W*Q  +. 
XVP*V*P+XVR*V*R+U*Q* { XQDS*DS+XQDB/ 2*DBP ) +XRDR*U*R*DR >  1- . . 
(RHO/2}*L**2*(XVV*V**2  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W*.. 
(XWDS*DS+XWDB/2*DBP)+U**2*(XDSDS*DS**2+XDBDB/2*DBP**2+ . . 
XDRDR*DR**2))- (WEIGHT  -BOY) *SIN( THETA)  +(RH0/2 )*L**3*  .. 
XQDSN*U*Q*DS*EPS+(RH0/2)*L^*2*(XWDSN*U*W*DS+XDSDSN*U**2* 


HsastasMaSitiSisaiS^^ 


*  *  *  *  *  *  ***  *  *  *  *  *  *  ********  to*  *  *  * 


DS**2)*EPS  +(RH0/2) *L**2*U**2*XPROP+RHO/2*L**3*U*Q*  ... 
X0DB/2*DBS  4-RHO/2*L**2*U**2*XDBDB/2*DBS**24-  . . . 
RflO/2*L**2*XWDB/2*DBS*U*W 

sway  FORCE 

FP<2)  *  -MASS*U*R  +  MASS*XG*P*Q  4-  MASS*YG*R**2  -  MASS*ZG*Q*R  +... 
(RH0/2)*L**4*(YPQ*P*Q  4-  YQR*Q*R)4.(RHO/2)*L**3*(YP*0*P  4-... 
YR*U*R  4-  YVO*V*Q  4-  YWP*W*P  +  YWR*W*R)  +  (RHO/2)*L**2*  ... 

(YV*U*V  +  YVW*V*W  +YDR*U**2*DR)  -LATYAW  4-(WEIGHT-B0Y)*.  .  . 
COS (THETA) *S IN (PHI) 

heave  FORCE 

FP(3)  ■  MASS*U*0  -  MASS*V*P  -  MASS*XG*P*R  -  MASS*YG*0*R  + 

MASS*ZG*P**2  +  MASS*ZG*0**2  +  (RH0/2)*L**4*(ZPP*P**2  +. . . 
ZPR*P*R  +  ZRR*R**2)  +  (RHO/2)*L**3*7ZQ*U*Q  4*  ZVP*V*P  +... 
ZVR*V*R)  4-7rHO/2)*L**2*(ZW*U*W  +  ZVV*$**2  +  U**2*(ZDS*. . . 
DS+ZDB/2*DBP))-n6rPIT+(WEIGHT-B0Y)>i'C0S(THETA)*C0S(PHI)+.  . . 
(RHO/2)*L**3*ZQN*U*Q*EPS  +{RH072j*L**2*(ZWN*U*W  +ZDSN*.  .  . 
U**2*DS ) *EPS+  RH0/2*L**2*U**2*2!DB/2*DBS 

ROLL  FORCE 

FP(4)  *  -IZ*Q*R  +IY*Q*R  -IXY*P*R  +IYZ*Q**2  -IYZ*R**2  +IXZ*P*Q  +... 
MASS*?G*U*Q  -MASS*YG*V*P  -MASS*ZG*W*P4-(RHO/2)*L**5*(KPQ*. .  . 
P*Q  +  KQR*Q*R)  +(RH0/2)*L**4*(KP*U*P  +KR*U*R  +  KVQ*V*Q  +... 
KWP*W*P  +  KWR*W*R)  4-(RH0/2)*L**3*(KV*U*V  +  KVW*V*W)  +  ... 

(YG* WEIGHT  -  YB*B0Y V*COS (THETA )*C0S( PHI)  -  (ZG*WEIGHT  -  ... 
ZB*B0Y)*C0S(THETA)*SIN7pHI)  +  (RHO/2)*L**4*KPN*U*P*EPS  +... 
7RHO/2 j  *L**3*U**2"KPROP  +MASS*ZG*U*R+  ... 
RHO/2*L**3*U**2*(KDB/2*DBP-KDB/2*DBS) 

PITCH  FORCE 


FP(5)  -  -IX*P*R  +IZ*P*R  +IXY* 


4-IXZ*R**2  -... 


XB*BOY 
(RHO/2. 
U**2*MDB/2*DB 


*DBP))+  NORPIT  -(: 

,  + . . . 
Mj**2*DS)*EPS+  RH0/2*L**3*. . . 
■  ( ZG*WEIGHT-ZB*BOY ) *S IN ( THETA ) 


YAW  FORCE 

FP(6)  «  -IY*P*Q  +IX*P*Q  +IXY*P**2  -IXY*Q**2  +IYZ*P*R  -IXZ*Q*R  -... 
MASS*XG*U*R  4-  MASS*XG*W*P  -  MASS*YG*V*R  +  MASS*YG*W *0  +... 
(RHO/2 )*L**5*(NPQ*P*0  +  NQR*Q*R)  4-(RH0/2)*L**4*(NP*U*P4- .  . . 
NR*U*R  +  NVQ*V*Q  4-NWP*W*P  +  NWR*W*R)  4-(RHO/2)*L**3*7NV*.  . . 
U*V  4-  NVW*V*W  4-  NDR*U**2*DR)  -  LATYAW  4-  (XG* WEIGHT  -  ... 

XB*B0Y)*C0S(THETA)*SIN(PHI)4-(YG*WEIGHT)*SIN(THETA) . .  . 
4-(RHO/2)*L**3*U**2*NPROP-YB*BOY*SIN(THETA) 


00 


IF(Z.E0.50.0)THEN 

WRITE  (8,500)(FP(I) ,  I  =  1,6) 

Z  »  0.0 
END  IF 

NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  =  1,6 

F(J)  *  0.0 
DO  600  K  =  1,6 

=  MMINV(J,K)*FP(K)  4-  F(J) 

CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 
FIRST  SET  THE  DRIFT  CURRENT  VALUES 
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-IC-fc-IC  -K  ««««««  «««««««  *  *  *  *  *  O*  -K 


iwvwwwwff  jmi  wwiF.nrfWvv'  nr  -■ 


UCO  ■  0.0 
VCO  ■  0.0 
WCO  ■  0.0 

INERTIAL  POSITION  RATES  F(7-9) 

F(7)  ■  UCO  +  U*C0S ( PS  I ) *C0S (THETA )  +  V*(C0S(PSI )*SIN (THETA)*. . . 

SIN(PHI)  -  SIN(PSI)*C0S7pHI))  +  W*(COS(PSI)*SIN(THETA)*. . . 
COS (PHI;  +  SIN(PSI;*SIN(PHl) ; 

F(8)  ■  VCO  +  U*SIN(PSI)*COS (THETA)  +  V*(SIN(PSI)*SIN(THETA)*. . . 

SIN(PHI)  +  C0S?PSI)*C0S(PHI))  +  W*(SIN(PSI)*SIN(THETA)*. . . 
COS (PHI;  -  COS(PSI;*SIN(PHl) ; 

F(9)  ■  WCO  -  U*SIN (THETA)  +V*C0S (THETA)*SIN(PHI )  +W*C0S ( THETA )*.. . 
COS (PHI) 

EULER  ANGLE  RATES  F( 10-12) 

F(10)  «  P  +  Q*S IN ( PHI )*TAN (THETA)  +  R*C0S ( PHI )*TAN( THETA) 

F(ll)  «=  Q*C0S(PHI)  -  R*SIN(PHI ) 

F(12)  =  Q^S IN (PHI) /COS (THETA)  +  R*C0S( PHI) /COS (THETA) 


iMZ-f Q.1.°)WRITE  (9,500)  (F(I) ,  I  =  1,12) 
FORMAT (6E12 .4) 

Z  =  Z  +  1 


UDOT  =  F( 1 ) 

VDOT  *  F(2) 

WDOT  a  F(3) 

PDOT  =  f(4) 

QDOT  a  f(5) 

ROOT  a  f(6) 

XDOTA=  F(7) 

YDOT  =  F(8) 

ZDOT  =  F(9) 

PHIDOT  a  f ( 10 ) 

THETAD  =  f(11) 

PSIDOT  a  f(12) 

U  »  INTGRL( 6.0, UDOT) 

V  »  INTGRL( 0.0, VDOT) 

W  =  INTGRL( 0.0, WDOT) 

P  =  INTGRL ( 0.0, PDOT ) 

6  =  INTGRL( 0.0, QDOT) 

R  =  INTGRL( 0.0. ROOT) 

XPOS  =  INTGRL (0.0, XDOTA ) 
YPOS  =  INTGRL ( 0.0, YDOT) 

ZPOS  =  INTGRL ( 0.0) ZDOT ) 

PHI  =  INTGRL(0.0, PHIDOT) 
THETA  =  INTGRL (0.0, THETAD) 
PSI  =  INTGRL (0.0, PSIDOT) 

7UITU  ;  »7pAC 

PHIANG  =  PHI/0.0174532925 
THEANG  =  THETA/0.0174532925 
PSIANG  =  PSI/0. 0174532925 


UM0D(4)»  15.0*0.0174532925 
UMOD ( 3 )  ■  -15.0*0.0174532925 
UMOD ( 2 )  ■  -15.0*0.0174532925 

END  IF 

IF(IFLAG.EO.O. AND. ABS(THMANG).GT. 37.0)  THEN 
ZCHO  *  x(9)  -  5.0 

I  FLAG  -  I  FLAG  +  1 

END  IF 

IF  (IFLAG. GT .0.0 .AND . JFLAG . EQ . 0 )  THEN 
UM0D(4)«  2.05*0.0174532925 
UMOD (  2 )  «  0.0 
UMOD ( 3 )  »  0.0 

END  IF 

IF  (IFLAG. GT.O. AND. ZCHO. GT.ZDEPTH)  THEN 
UM0D(4)«  -11.0*0.0174532925 
UMOD ( 3  j  »  11.0*0.0174532925 
UMOD ( 2 )  -  11.0*0.0174532925 

END  IF 

IF  (ZDEPTH. LT. 3 .0. AND. ABS (THMANG) .LT.4. 10)  THEN 
UM0D(4)=  0.0 
UMOD ( 3 }  »  0.0 
UMOD(2)  *  0.0 
JFLAG  *  JFLAG  +  1 

END  IF 

IF  (JFLAG. GT.O)  THEN 
UM0D(4)=  0.0 
UMOD (  3 )  ■  0.0 
UMOD (2)  ■  0.0 

ENDIF 


CONTROL  FINTIM  «200.00,DELT  »  .01 

SAVE  . 20 , XPOS , XPOSM , U , UM , ZPOS , ZPOSM , W ,  WM , DBPM .... 


PRINT  2 


v  t  vw  t  ac  »  w  t  wu  ,  vrf  ,  ucwgu  ,  n  ,  nu  .  uv 

DBS , DBSM.DS , DSM , THEANG , THMANG ,  Q ,  OM 
. 0 . XPOS  .XPOSM ,  U ,  UM , ZPOS  .  ZPOSM ,  W ,  WM , DB: 


C  A  ,  ACVW  i  ACWUli  >  W  .  WU  ,  (JC  Vd  .  f  tJ  .  TJ 

DBS , DBSM . DS , DSM . THEANG , THMANG , 
GRAPH  (G1,DE=TEK618)  TIME(NI=10.UN»SEC) 

ZPOSM (LI=2,UN=FT) 

GRAPH  (G2,DE*TEK618)  TIME (NI»10 ,UN=SEC) 


m 


BPM , .  .  . 


S(LI=1.UN=FT>... 


GRAPH  ( G3 , DE=TEK6 18)  TIME(NI=10 ,UN=SEC)  6 

QM(LI=2.UN='RAD/SECf 
GRAPH  (G4,DE=TEK618)  TIME (NI=10 ,UN=SEC)  T 

THMANG (LI=2 , UN=DEG ) 

GRAPH  (G5,DE=TEK618)  XPOS(UN=FT)  ZP0S(UN= 


NI=10,UN=SEC)  THEANG (LI=1 , UN=DEG) . 


GRAPH 

GRAPH  (G7;DE=TEK618)  TIME(l)l=10  ,UN=SEC) 'DBS(Lt=l  ,UN=RADIANS) 

DBSM(LI=2 ,UN=RADIANS ) 

GRAPH  (G8,DE=TEK618)  TIME(NI=10 ,UN=SEC)  DS  (LI=1 ,UN=RADIANS) 

DSM(LI=2,UN=RADIANS) 

LABEL  (G1 ,DE=TEK618)  (DEPTH  VS  TIME) 

LABEL  (G2,DE=TEK618)  (HEAVE  VS  TIME) 

LABEL  (G3,DE=TEK618)  (PITCH  RATE  VS  TIME) 

LABEL  G4 , DE=TEK6 18)  (PITCH  ANGLE  VS  TIME) 


G6,DE=TEK618 

G7,DE=TEK618 


( UN=FT )  ZPOS ( UN=F1 
M(UN=FT)  ZPOSM (UN= 
;(NI=10,UN*SEC)  DBS 


!,i=l,UN= 


RADIANS) 


LABEL  (G1,DE=TEK618 
LABEL  (G2,DE=TEK618 
LABEL  (G3.DE*TEK618 
LABEL  (G4,DE=TEK618 
LABEL  i,G5,DE=TEK618 
LABEL  iG6,DE=TEK618 
LABEL  \G7,DE=TEK618 
LABEL  (G8,DE=TEK618 
END 


ACTUAL  DIVE  PROFILE 
MODEL  DIVE  PROFILE) 
BOW  PLANE  ANGLE) 
(STERN  PLANE  ANGLE) 


******SUBROUTINE  ETAT**********5''********************************51' ******** 

************************************************************************* 

PROGRAM  ETAT 
IMPLICIT  REAL*8(A-H,0-Z) 

COMMON/SYST/ A (40 , 40 ) , B 7 40 , 40 ) , C ( 40 , 40 ) , D (40 , 40 ) 

COMMON/STATES/X(40) ,Y(40) ,U(46) ,W(40) 

COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIM1/DT 
COMMON/FLAGS/ IOPT ( 5 ) 

COMMON/OPTIM/Q ( 24 , 24 ) , R ( 24 , 24 ) 

DATA  EPS/1. OE-7/ 


C 

C 

C 

C 


10 


15 


16 

C 

C 

c 


INITIALIZE  ALL  SYSTEM  MATRICES 

EPS  =  1 .OE-7 

DO  10  1=1,40 

X(I)=0.0 

Y(I)=0.0 

U(I)=0.0 

CONTINUE 

DO  15  1=1,40 

DO  15  J=1 ,40 

A(I, J)=0.0 

B(I,J)=0.0 

C(I,J)=0.0 

D(I,J)=0.0 

CONTINUE 

DO  16  1=1,24 

DO  16  J=1 , 24 

g(I,J)=0.6 

fi(i,J)=o.o 

CONTINUE 

SET  UP  COEFICIENT  MATRICES, INPUTS, INITIAL  CONDITIONS 


CALL  OPTIMA 
CALL  POUTOP 


CALL  INPUT 
CALL  MTXEXP 
CALL  ROOTS 
DO  20  K=1 ,NKS 
CALL  EXCIT(K) 

CALL  UPDAT(K) 

CALL  POUT(K) 

20  CONTINUE 

IF  (IOPT(l).EQ.l) 

C  IF  (IOPT(l).EQ.l) 

END 

A*yrfe**guBj^oUTINE  lNPUT**’if,*::*!****!,t**,,t,,t,,c*,,t***:^******,,t**‘*:5lt***:*'***:*t'*:,lc*,*t,,c’'r,';:*:,':** 

*  A  ******************  **  jit  *  A  i*c  A  *  A  *1*:  A*  A  A  i*c  **********  A  ****  A*****  ******  *  *  *  rt  * 

SUBROUTINE  INPUT 
IMPLICIT  REAL*8(A-H,0-Z) 

COMMON/SYST/A(40 ,40) ,B(40 ,40) ,C(40 , 40) ,D(40 ,40) 

COMMON/STATES/X(40) ,Y(40) ,U(40) ,W(40) 

COMMON/DIM/N , M , NR , NKS , EPS 

COMMON/DIM1/DT 

COMMON/ FLAGS / I OPT ( 5 ) 

COMMON/OPTIM/Q(24 , 24) ,R(24 , 24) 

C  OPEN(UNIT=5 , f!LE= 1  FILE ' , STATUS= 1  OLD ' ) 

C  0PEN(UNIT=6 , FILE= 1  FILE  1 , STATUS= 1  OLD ' ) 

READ  (5,10)  N,L,M,K,NKS,I0PT(1) ,DT 
WRITE  6,10)  N,L,M,K,NKS,I0PT(1) ,DT 
READ  IS^y  NAS 

WRITE (5, 9)  NAS 
DO  11  11=1. NAS 
READ  (5,25)  I,J,A 
WRITE (6 , 25)  I , J, A 
11  CONTINUE 

READ  (5,9)  NBS 
WRITE (6, 9)  NBS 
DO  12  11=1, NBS 


ii:3) 
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i  ir»  irw  r*  \rr  it* v»  vkuh  u*  vw  vvuirvirvYinrw'W  i%^wvwwwmi 


12 


13 


14 


35 


)  I, J ,B(I, J) 
>  I, J ,B(I , J) 


READ  (5,25)  I,J,B(I,J 
WRITE (6, 25)  “  “ 
CONTINUE 


READ  (5,9 
WRITE(6  ~ 
DO  13  II 


NCS 

NCS 

NCS 


WV  A  A  A — ^  itiww 

READ  (5,25)  I,J,C(I,J) 
WRITE (6^25)  I,J,C(I,J) 


CONTINUE 
READ 
WRITE 
DO  14 


NDS 

NLS 

NDS 


ww  a.  m  —  j.  >iiww 

READ  (5,25)  I,J,D(I,J) 
WRITE (6,25)  I, J,D(I, J) 


CONTINUE 
READ^ 5,9)  NXS 
WRITE (6, 9)  NXS 
DO  35  I 1*1, NXS 


WRITE (6, 25) 
CONTINUE 
IF(IOPT(l).NE.l) 
READ (5 ,9)  NOS 


GO  TO  190 


150 


180 

9 

10 
25 
190 


WRITE (6, 9)  NOS 
DO  150  11=1, NOS 
READ(5 ,25)  I,J,Q(I,J) 

write (6,25)  :,j;q(i,j) 

CONTINUE 
READ (5, 9)  NRS 
WRITE (6, 9)  NRS 
DO  180  11=1 ,NRS 
READ(5 ,25)  I,J,R(I,J) 

WRITE(6,25)  I,J,R(I,J) 

CONTINUE 
NR=L 

FORMAT (5X, 15) 

FORMAT (5X, 615, E10. 4) 

FORMAT  (I5,I5,E10.4) 

RETURN 
END 

******subroutine  MTXEXP***********************************”************** 
************************************************************************* 

SUBROUTINE  MTXEXP 
IMPLICIT  REAL*8(A-H,0-Z) 

COMMON/SYST/A(40 ,40 ) , B (40,40) ,C(40,40) ,D(40, 40) 

COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIM1/DT 

COMMON/hTT/E(40,40) ,H(40.40) 

DIMENSION  DD(40,40) ,L(50) ,RH0(50,2) ,W(50) 

MK=30 

DO  20  1=1, N 
L(I)=1 

RHO( I , 1 )=1 .0 
DO  20  J=1,N 

IF(I.EQ.J)  GO  TO  10 
E(I,J)=0.0 
DD(I,J)=0.0 
H(I,J)=0.0 
GO  TO  20 
10  CONTINUE 

E(I, J)=1.0 
DD(I , J)=l ,0 
H(I,J)=DT 
20  CONTINUE 

MM=0 
K=1 
X=DT 

30  CONTINUE 

DO  80  1=1, N 
IF(L(I).EQ.0)  GO  TO  80 
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GO  TO  60 


40 


50 

60 


70 

80 


DO  40  J=1,N 
W(J)=0.0 
CONTINUE 
DO  60  KK=1 ,N 
Y=DD(I,KK) 

IF(Y.EQ.O.O) 

DO  50  J=1,N 

MI)+y*i(KK’J) 

CONTINUE 
DO  70  J=1 ,N 
DD(i,jj=W(J)*X 
CONTINUE 
CONTINUE 
K=K+1 
X=K 

X=DT/X 

DO  100  1=1, N 
IF(L^.EQ.O)  GO  TO  100 

DO  90 ' J=1 ,N 

-‘=e(I, J)+DD(I,J) 

=h(i,j)+x*dd(i,j) 

P+ABS(E(I, J); 

90  CONTINUE 

RHO(I ,2)=Y1P 

IF^ABS( (RHO(I ,2) -RHO(I , 1) )/RHO(I ,2) ) .GT.EPS)  GO  TO  100 
MM=MM+1 

100  CONTINUE 

IF(K.GT.MK)  GO  TO  130 
IF(MM.EQ.N)  GO  TO  120 
DO  110  1=] ,N 
RHO (1,1) =RHO (1,2) 

110  CONTINUE 
GO  TO  30 
120  CONTINUE 

DO  125  1=1, N 
DO  125  J=1,N 
DD(I , J)=0.0 
DO  125  K=1,N 

DD(I,J)=DD(I,J)+H(I,K)*B(K,J) 

125  CONTINUE 

DO  135  1=1, N 
DC  135  J=1,N 
H(I , J)=DD(I , J) 

135  CONTINUE 

WRITE  (6,190) 

190  FORMAT (5X, 'P-MATRIX' ,/) 

WRITE (6 ,200)  ( (E (I , J) , J=1 ,N) , 1=1 ,N) 

WRITE  (6,195) 

FORMAT ( 5X , 1 QB-MATRIX 1 , / ) 

WRITE(o , 200)  ( (H(I , J) , J=1 ,N) , 1=1 ,N) 

F0RMAT(6E12.4) 

RETURN 
CONTINUE 

WRITE (6, 140)  MK  ,EPS 
STOP 

140  F0RMAT(1X, 'MATRIX  EXPONENTIAL  FAILED  TO  CONVERGE  AFTER  ',14, 

1'  ITERATIONS' ,/, IX, 'CONVERGENCE  FACTOR ' ,E12 .4) 

END 

******5UBROUTINE  ROOTS *•*•' **)•(*** ************** *******5^***********:*:******** 
Aifcik*  ***********************  Art*  A  hAA******!**^!***  Art***  A  A***  A************* 

SUBROUTINE  ROOTS 
IMPLICIT  REAL*8(A-H,0-2) 

C0MPLEX*16  ZZ 

COMMON/SYST/A (40 , 40 ) ,B(40,40) ,C(40,40) ,D(40,40) 

COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/ DIM1/DT 

DIMENSION  W(80) ,ZZ(40 ,40) ,WK(3200) 
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■  uvuBwvuvuwini  urvTViftuv 


10 


15 


20 


25 

4 

3 


8 


•DIMENSION  XX(40,40),RZ(3200) 

EQUIVALENCE  (ZZ(1,1),  RZ(1)) 

I J0B=2 
IZ=40 
IA*40 

DO  10  1=1,3200 
WK(I)=0.0 
CONTINUE 
DO  15  1=1,80 
W(I)=0.0 
CONTINUE 
DO  20  1=1,40 
DO  20  J=1 ,40 
ZZ(I,jJ=0.0 
CONTINUE 
DO  25  1=1, N 
DO  25  J=1,N 
XX(I,J)=A(I,J) 

CONTINUE 
DO  4  I  =  1,N 

WRITE?6,3)(XX(I,J),  J=1,N) 

FORMAT (6E12. 4) 

CALL  EIGRF (XX,N, IA, IJ0B ,W,RZ, IZ , WK, IER) 

WRITE (6 , 8 ) ( W ( I ) ^  I  =  l,60j 
FORMAT (4E12. 4) 

N2=N*2 

DO  30  1=1, N2, 2 
W1=W(I) 

11=1  +  1 
W2*W(I1) 

I2=(I+l)/2 

WRITE (6, 100)  12 ,W1 ,W2 
DO  50  1=1, N 
DO  40  J=1,N 
XX(l,J)=REAL(ZZ(J,n) 

XX(  2 , J)=DIMAG(ZZ ( J , 1 ; ) 

WRITE  (6,120)  I,XX(l,j),XX(2,J) 

CONTINUE 

FORMAT (5X, 'EIGENVALUES'  10X' REAL  PART' 

11 OX, 'IMAGINARY  PART' ,/ , 5X, 13 , 12X,E12 .4 , 10X,E12 .4) 

FORMAT ( 5X , ' EIGENVECTORS  1 ,I5,5X,2E12.4) 

FORMAT(5X ,'IER  AND  PERFORMANCE  INDEX',15 , 10X,E12 .4) 

WRITE  (6,130)  IER,WK(1 ) 

RETURN 

END 

******gUBROUTINE  ****A******A********A^c********************** A*** 

************************************************************************* 

SUBROUTINE  EXCIT(K) 

IMPLICIT  REAL*8(A-H,0-Z) 

COMMON/SYST/A(40 ,40) ,B(40,40) .C(40 ,40) ,D(40,40) 

COMMON/ STATES/X(4Q) ,Y(40)  U(46) >(40) 

COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIM1/DT 
T=DT*FLOAT ( K ) 

U(1)=0.0 

RETURN 

END 

AAAAAAguBROUTINE  UPDAT  (k) ********************************************** 
************************************************************************* 
SUBROUTINE  UPDAT (K) 

IMPLICIT  REAL*8(A-H.O-Z) 
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40 

50 

100 

120 

130 


WAV**  MUOM  W\n  11/  V  U  I 

C0MM0N/SYST/A(40 ,40 ) , B(40 ,40) ,C(4Q , 40) ,D(40,40) 

COMMON/STATES/ X (40) ,Y(40) ,U(46) ,W(40)/ 

COMMON/DIM/N , M , NR , NKS , EPS 

C0MM0N/DIM1/DT 

COMMON/ ETT/E( 40. 40) ,H(40,40) 

DIMENSION  XS (40) ,XN(40) ,YS (40) , YN(40) 
T=DT*FLOAT(K) 

DO  10  1=1, N 
XS(I)=0.0 


=XN 
=YS 
,  )=YN 
10  CONTINUE 

DO  20  1=1, N 

^{aiisp 

TINUE 


*  f  ii 

(i)+E< 

I ,  J] 

*x< 

J 

(i)+h 

I,J 

*u 

J) 

i  +c 

*x 

J) 

(I)+D 

u 

*u 

J) 

20 


ii 


cffi! 

RETURN 
END 

******5UBR0UTINE  POUT(K)  ************************************************* 

kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk 

SUBROUTINE  POUT(K) 

IMPLICIT  REAL*8(A-H/0-Z) 

COMMON/ SYST/ A (40 ,  40 ) , B(40 , 40 ) , C (40 , 40 ) , D (40 , 40 ) 

COMMON/STATES/ X (40) , Y(4Q) ,U(46) ,W(40) ' 

COMMON/DIM/N , M , NR , NKS , EPS 
COMMON/DIM1/DT 
T=FLOAT(K)*DT 
IF(K.EQ.l)  WRITE  (6,110) 

‘  ?),Y(3),Y(4) 


100 

110 


T,Y 


M 


> 


WRITE  (6,100) 

FORMATi 5X,F10 .2, 5X, 

FORMAT ( -  '■ 

1,10X, 

RETURN 
END 

******subroutine  optima************************************************** 


'TIME' ,5X, 'XI' ,5X, ' K2 ' ,5X, 'X3' ,5X, 'X4‘ ,5X, ' Ul ' 
12 '  ,  / ) 


THIS  SUBROUTINE  SETS  UP  THE  SYSTEM  AND  ADJOINT  EQUATIONS 
MATRICES  AS 

A  -(B(R-l)BT) 

SS  = 

-0 .  -AT 

AND  FINDS  THE  EIGENVALUES  /EIGENVECTORS  OF  SS. 

COLLECTING  THE  STABLE  VECTORS  AS  IN  POTTERS  METHOD 
AND  PARTITIONING, RESULTS  IN  THE  SOLUTION  OF  THE 
RICCATI  EQUATION  FOR  THE  OPTIMUM  STATE  FEEDBACK 
GAINS. THIS  ROUTINE  LIMITS  A (24, 24). 


* 

* 
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k 

k 

k 

k 

k 
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SUBROUTINE  OPTIMA 
IMPLICIT  REAL*8(A-H,0-Z) 

C0MPLEX*16  ZZ,W12,W22 

COMMON/ SYST/ A (40, 40) ,B(4u,40) ,C(40,40) ,D(40,40) 

COMMON/OPTIM/Q ( 24 , 24 ) , R ( 24 , 24 
COMMON/DIM/N, M, NR, NKS, EPS 
COMMON/DIM1/DT 

DIMENSION  SS (48 ,48) .TEMP (24 ,  24 ) , ZZ(48 , 48) , W(96) ,WK(2400) 

DIMENSION  W12(24 , 24) ,W22 (24 , 24)  ,P.Z(4608) 

EQUIVALENCE  (ZZ(1 , 1) ,RZ( 1 ) ) 

IZ=48 
I JOB=l 
N2=2*N 
N4=2*N2 
N21=N2+1 
DO  1  1=1, N2 
DO  1  J=1 ,N2 
1  SS(I,J)=0.0 

DO  5  1=1, N 
DO  5  J=1.N 
5  TEMP(I,J)=0.0 

C  WRITE (6, 140) 

NDIM1=24 
NDIM2=48 

CALL  INVERT (R , DET , NR , NDIM1 , NDIM2 ) 

C  WRITE  6,150)  ( (R(I , J) , J=1 ,NR) , 1=1 ,NR) 


( (R(I , J) , J=1 ,NR) , 1=1 ,NR) 
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DO  10  1=1, NR 
DO  10  J=1,N 
DO  10  K=1 ,NR 

TEMP(I , J)=TEMP(I , J)+R(I ,K)*B( J,K) 

DO  20  1=1, N 

DO  20  J=1,N 

JJ=J+N 

DO  20  K=1,NR 

SS (I , JJ)=SS(I , JJ)-B(I ,K)*TEMP(K, J) 
DO  30  1=1, N 
DO  30  J=1,N 
SS(I.J)=A(I,J) 

DO  40  1=1, N 
DO  40  J=1,N 
II=I+N 
JJ=J+N 


SS(II,JJ)=-A(J,I) 
CALL  EIGRF(SS,N2,IA, 


WRITE (6, 90) 

DO  50  1=1  ,N4,; 

I2=(I+l)/$ 

W1=W(I) 

11=1+1 
W2=W (II) 
WRITE(6,120) 


IJOB,W,RZ,IZ,WK, IER) 


W2=W(I1) 
WRITE(6,120) 
WRITE (6, 100 ) 
DO  70  J=1 ,N2 
DO  60  1=1, N2 


I2,W1,W2 


WRITE  (6,120)  J,SS(I , i) ,SS(I , 2) 

CONTINUE 

CONTINUE 

COLLECT  ALL  STABLE  EIGENVECTORS  INTO  A  V-MATRIX 
(USING  SS (48, 48)) .PARTITION, AND  SOLVE  FOR  THE 
SOLUTION  OF  THE  RICCATTI  EQUATION  . 

J=0 

DO  210  IC=1,N4,2 
JC=(IC+l)/2 

IF(W(IC) .GE.0.0)  GO  TO  210 


JC) 

N,  JC) 


J=J+1 

DO  200  1=1, N 
IPN=I+N 

W12 (I , J)=ZZ(I , JC) 

W22(I , J)=ZZ(IPN, JC) 

CONTINUE 

CONTINUE 

INVERT  COMPLEX  W12(N,N) 

DO  220  1=1, N 
DO  220  J=1,N 
IPN=I+N 
JPN=J+N 

SS(I, J)=REAL(W12(I , J) ) 

SSiIPN, J)=DIMAG(W12(I , J)) 
SS(I,JPN)=-SS(IPN,J) 

SS (IPN , JPN ) =SS ( I , J ) 

CONTINUE 

NDIM1=48 

NDIM2=96 

CALL  INVERT(SS,DET,N2,NDIM1,NDIM2) 
FORM  W22*(W12)-1=P 

DO  230  1=1, N 
DO  230  J=1,N 
IPN=I+N 
Q(I,J)=SS(I,J) 

R(I.J)=SS(IPN,J 
DO  240  1=1, N 
DO  240  J=1 ,N 

ss(i,j)=o.6 


SS(IPN, J) 
=SS(I, J) 


DO  240  K=1,N 

SS(I,J)=SS(I,J)+REAL(W22(I,XV''-'Q(K,J)-DIMAG(W22(I,K))*R(K,J) 

240  CONTINUE 

C  FORM  GAIN  MATR’  ,.  INTO  THE  Q  ARRAY 

DO  250  1=1, NR 
DO  250  J=1 ,N 

8(i,j)=P<i,j)+temp(i,k)*ss(k,j) 

ONTINUE 

C  COMPUTE  THE  CLOSED  LOOP  A-MATRIX 

DO  260  1=1, N 
DO  260  J=1,N 
DO  260  K=1 ,NR 

260  5&Ei^7X6f-B<I'K)^<K'J) 

DO  265  K=1,NR 

265  WRITE (2 ,275)  (Q(K, J) ,J=1,N) 

C  WRITE (6, 280 

C  DO  285  1=1 ,N 

C85  WRIT£(6 , 275)  (A(I , J) , J=1 ,N) 

C  CALL  ROOTS 

90  FORMAT (5X, 1 EIGENVALUES-SYSTEM+ADJOINT- • ) 

100  FORMAT (5X,  'EIGENVECTORS  RE/IMAG') 

120  FORMAT(5X,I5,10X,E12.4,10X,E12.4) 

150  FORMAT(5X, 'R-INVERSE' ,/,4E12.4) 

140  FORMAT(5X, 'R-MATRIX' ,/,4E12.4) 

270  FORMAT (5X, 'TOTAL  STATE  FEEDBACK  GAIN  MATRIX',/) 

275  FORMAT (3E 20. 10) 

280  FORMAT (5X, 'CLOSED  LOOP  A-MATRIX',/) 

RETURN 

END 

******gUg{^QUipjjjg  INVERT************************************************** 

******* ** *************** A************************************************ 
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SUBROUTINE  INVERT ( A , DET , N , NDIM1 , NDIM2 ) 

IMPLICIT  REAL*8(A-H,0-Z) 

THIS  ROUTINE  INVERTS  A  SQUARE  MATRIX  USING 
GAUSS  ELIMINATION. THE  ORIGINAL  MATRIX  IS  DESTROYED 
AND  ITS  INVERSE  IS  RETURNED  AS  1 A ' . 

DIMENSION  A(NDIM1 ,NDIM2) 

NDIGIT=30 
SUM=0 .0 


DO  10  1=1, N 
DO  10  J=1,N 
SUM=SUM+ABS ( A ( I , J ) ) 

CONTINUE 

SUM=10 . 0** ( -NDIGIT/2 . ) *SUM/N**2 

NP1=N+1 

NPN=N+N 

DO  20  1=1, N 

IPN=I+N 


DO  20  J=NP1 ,NPN 
A(I,J)=0.0 

IF  (I-’N.EQ.  J)  A(I ,  J)=l  .0 

CONTINUE 

DO  25  1=1, N 

WRITE (6 , 900)  <A(I,J),J=1,NPN) 

CONTINUE 

DET=1 . 

INTCH=0 
DO  90  1=1, N 
IP1=I+1 


IF  (I.EQ.N)  GO  TO  50 
M=I 


DO  30  J=IP1,N 

IF  (ABS (A(M, I ) ) .LT.ABS (A(J,I)))  M=J 
CONTINUE 

IF  (M.EQ.I)  GO  TO  50 
INTCH=INTCH+1 
DO  40  J=1 ,NPN 


TEMP=A(M, J) 

ifepfcJ) 

CONTINUE 

CONTINUE 

IF(A(I,I).EQ.0.0 
IF  <Ab£<A(I,I)). 
DO  60  J=IP1 ,NPN 


0)  GO  TO  110 
.LT.SUM)  WR 


DO  60  J=IP1,NPN 
A(I,J)=A(I,J)/A(I,I) 

CONTINUE 
DO  80  J=1,N 
IF  (J.EQ.I)  GO  TO  80 
DO  70  K=IP1,NPN 

AI'kR“A<J'K>"A<J'I>*A<I'K> 

CONTINUE 

A(J,l)=0.0 

CONTINUE 

DET=DET*A(I,I) 

CONTINUE 

DET= (  - 1 ) **INTCH*DET 
DO  100  1=1,  N 
DO  100  J=1  ,N 
A(I , J)=A(I, J+N) 

CONTINUE 
DO  26  1=1, N 

WRITE (6 ,910)  (A(I, J) ,J=1,NPN) 

CONTINUE 


WRITE (6 ,140) 


RETURN 

110  CONTINUE 

WRITE (6, 130) 

DO  120  1=1, N 
DO  120  J=1,N 
A(I,J)=1.0 
120  CONTINUE 
RETURN 

130  FORMAT (5X, 'THE  MATRIX  IS  SINGULAR, NO  SOLUTION  HAS  BEEN  FOUND') 

140  FORMAT (5X, 'THE  MATRIX  IS  ILLCONDITIONED ' ) 

900  FORMAT(2X, 'ABEF. ‘ ,6E12.3) 

910  FORMAT(2X, 'AAFT. 1 ,6E12.4) 

END 

******5UBROUTIME  POUTOP************************************************** 
************************************************************************* 


SU"  5UTINE  POUTOP 

TV  .ICIT  REAL*8(A-H,0-Z) 

RETURN 

END 


Bane,  G.L.,  Ferguson,  J. ,  "The  Evolutionary  Development 
of  the  Military  Autonomous  Underwater  Vehicle, »5th 


P fit * i K?) i t-.f 1 bit'll 


raftl  l  Mil  M  I  III  PM  j'l  I  I  M  MSij  I  Mil  u 


June  22-24,  1987,  Symposium  Proceedings. 


ered 


mpshire 


Herbert,  M. ,  Thorpe,  C.E.,  et  al.,  "A  Feasibility  Study 
for  a  Long  Range  Autonomous  Underwater  Vehicle,"  5th 


IS3  &!=}££•! 


,  University  of  New  Hampshire, 
June  22-24,  1987,  Symposium  Proceedings. 

NCSC  Technical  Memorandum  231-78,  "SDV  Simulator 
Hydrodynamic  Coefficients,"  by  N.S.  Smith,  J.w.  Crane 
and  D.C.  Summey,  June  1978. 

International  Business  Machiens  Program  Number  5798-PXJ, 
"Dynamic  Simulation  Language/vs  Language  Reference 
Manual,"  June  1984. 

NSRDC  Report  2510,  "Standard  Equations  of  Motion  for 
Submarine  Simulation,"  by  M.  Gertler  and  G.R.  Hagen, 
June  1967. 

Walker.  F.L. .  Analysis  and  Simulation  of  An  Autonomous 


Control  System.  Master's  Thesis, 
W222137,  Naval  Postgraduate  School,  Monterey, 
California,  June  1983. 

Amerongen,  J.  Van,  Udink,  A.J.  Ten  Cate,  "Model 
Reference  Adaptive  Autopilots  for  Ships,"  Automatica . 
Great  Britain.  1975,  Vol.  ll,  pp.  441-449. 

9.  Milliken,  G.L. ,  Multivariate  Control  of  an  Underwater 
Vehicle,  Master's  Thesis,  MIT,  Cambridge,  Massachusetts, 
1984. 

10.  Kwakernaack,  h.,  Sivan,  R. ,  Linear  Optimal  Control 
Systems .  Wiley  Interscience,  1972. 


11.  Kaufman,  H.,  Berry,  P.,  "Adaptive  Flight  Control  Using 
Optimal  Linear  Regulator  Techniques,"  Automatics.  Vol. 
12,  Oxford,  1976,  pp.  565-576.. 

12.  Slotine,  J.J.E.,  Tracking  Control  of  Non-Linear  Systems 
Using  Sliding  Surfaces.  Ph.D.  Dissertation,  MIT, 
Cambridge,  Massachusetts,  May  1983. 

13.  Yoerger,  D.R.,  Slotine,  J.J.E.,  "Robust  Trajectory 
Control  of  Underwater  Vehicles,"  I.E.E.E.  Journal  of 
Oceanic  Engineering.  Vol.  OE-IO,  Number  4,  October  1985. 

14.  Landau,  I.D.,  "A  Survey  of  Model  Reference  Adaptive 
Techniques— Theory  and  Applications,"  Automatica.  Great 
Britain.  Vol.  10,  1974,  pp.  353-379. 


No.  Copies 
2 


1.  Defense  Technical  Information  Center 
Cameron  Station 

Alexandria,  Virginia  22304-6145 


2.  Library,  Code  0142  2 

Naval  Postgraduate  School 

Monterey,  California  93943-5002 

3.  'Chairman,  Code  69Hy  5 

Mechanical  Engineering  Department 
Naval  Postgraduate  School 
Monterey,  California  93943-5004 

4.  Professor  D.L.  Smith,  Code  69Sm  1 

Mechanical  Engineering  Department 

Naval  Postgraduate  School 
Monterey,  California  93943-5004 

5.  Professor  R.  McGhee,  Code  52Mz  1 

Computer  Science  Department 

Naval  Postgraduate  School 
Monterey,  California  93943-5004 

6.  Professor  R.  Christi,  Code  62Cx  1 


Electrical  and  Computer  Engineering 
Department 

Naval  Postgraduate  Schoo; 

Monterey,  California  93943-5004 

7.  Dr.  J.  Crane  1 

Head,  Hydrodynamics  Branch 

NCSC 

Panama  City,  Florida  32407-5000 

8.  Russ  Weneth,  Code  u25  1 

Naval  Surface  Weapons  Center 

White  Oak,  Maryland  20910 

9.  Paul  Heckman,  Code  943  1 

Head,  Undersea  AI  &  Robotics  Branch 

Naval  Ocean  System  Center 
San  Diego,  California  92152 


135 


.rw^rwvwwvwvWirvirvWVWW  W»VWW  V 


10.  Dr.  D.  Milne,  Code  1563  1 

DTNSRDC,  Carderock, 

Bethesda,  Maryland  20084-5000 

11.  RADM  6.  Curtis,  USW  PMS-350  1 

Naval  Sea  Systems  Command 

Washington,  D.C.  20362-5101 

12.  LT  Relle  L.  Lyman,  Jr.,  USN  Code  90G  1 

Naval  Sea  Systems  Command 

Washington,  D.C.  20362-5101 

13.  LT  Richard  Boncal,  USN  3 

Raynes  Neck  Road,  RFD  2 

York,  Maine  03909 

14.  Distinguished  Professor  G.  Thaler,  Code  62Tr  1 


Electrical  and  Computer  Engineering 
Department 

Naval  Postgraduate  School 
Monterey,  California  93943-5004 


15.  Library  '  1 

Maine  Maritime  Academy 

Castine,  Maine 

16.  CDR  V.  Cox  1 

Marine  Engineering  Department  { 

Maine  Maritime  Academy 
Castine,  Maine 

♦ 

17.  LT  Vincent  Squitegri  1 

SMC  2431 

Naval  Postgraduate  School 
Monterey,  California  93943-5000 

18.  LT  Vince  Stammetti  1 

SMC  1104 

Naval  Postgraduate  School 
Monterey,  California  93943-5000 


136 


i 

i 

* 


